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Abstract 

The  purpose  of  this  research  was  to  obtain  a  navigation  solution  that  used  real  data, 
in  a  degraded  or  denied  global  positioning  system  (GPS)  environment,  from  low  cost 
commercial  off  the  shelf  sensors.  The  sensors  that  were  integrated  together  were  a 
commercial  inertial  measurement  unit  (IMU),  monocular  camera  computer  vision  al¬ 
gorithm,  and  GPS.  Furthermore,  the  monocular  camera  computer  vision  algorithm 
had  to  be  robust  enough  to  handle  any  camera  orientation  that  was  presented  to  it. 

This  research  develops  a  visual  odometry  2-D  zero  velocity  measurement  that  is 
derived  by  both  the  feature  points  that  are  extracted  from  a  monocular  camera  and 
the  rotation  values  given  by  an  IMU.  By  presenting  measurements  as  2-D  zero  ve¬ 
locity  measurements,  errors  associated  with  with  scale,  which  is  unobservable  by  a 
monocular  camera,  are  properly  handled.  The  2-D  zero  velocity  measurements  are 
represented  as  two  normalized  velocity  vectors  that  are  orthogonal  to  the  vehicle’s 
direction  of  travel,  and  are  used  to  determine  the  error  in  the  INS’s  measured  ve¬ 
locity  vector.  This  error  is  produced  by  knowing  which  directions  the  vehicle  is  not 
moving  in,  given  by  the  2-D  zero  velocity  measurements,  and  comparing  it  to  the 
IMU-reported  direction  of  travel. 

The  performance  was  evaluated  by  comparing  results  that  were  obtained  when 
different  sensor  pairings  of  a  commercial  IMU,  GPS,  and  monocular  computer  vision 
algorithm  were  used  to  obtain  the  vehicle’s  trajectory.  Three  separate  monocular  cam¬ 
eras,  that  each  pointed  in  a  different  directions,  were  tested  independently.  Finally, 
the  solutions  provided  by  the  GPS  were  degraded  (i.e.,  the  number  of  satellites  avail¬ 
able  from  the  GPS  were  limited)  to  determine  the  effectiveness  of  adding  a  monocular 
computer  vision  algorithm  to  a  system  operating  with  a  degraded  GPS  solution. 
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REAL-TIME  IMPLEMENTATION  OF  VISION,  INERTIAL,  AND  GPS 
SENSORS  TO  NAVIGATE  IN  AN  URBAN  ENVIRONMENT 


I.  Introduction 

This  thesis  outlines  the  research  efforts  that  were  used  to  pair  low  cost,  commercial- 
off-the-shelf  sensors  to  provide  a  navigation  solution  for  a  ground  vehicle  in  a  GPS- 
degraded  environment.  Sensors  that  were  used  in  this  research  were  a  commercial 
grade  inertial  measurement  unit  (IMU),  global  positioning  system  (GPS),  and  a 
monocular  camera  computer  vision  algorithm.  Motivation  behind  this  thesis  was 
to  provide  a  navigation  platform  that  could  operate  off  of  low  cost  sensors  when 
a  degradation  of  the  GPS  was  encountered.  While  the  implementation  of  a  paired 
monocular  camera  computer  vision  algorithm  with  an  IMU  and  GPS  is  not  a  new 
concept,  the  development  of  the  monocular  camera  computer  vision  algorithm  being 
used  as  a  2-D  zero  velocity  update  with  real  data  in  this  research  is.  Furthermore, 
the  implementation  of  the  monocular  camera  computer  vision  algorithm  in  a  GPS 
degraded  environment  shows  merit  of  the  sensor  as  a  viable  aid  when  operating  in  an 
urban  terrain. 

Since  the  launch  of  the  first  satellite  in  1978  [21],  GPS  has  provided  more  accurate 
worldwide  navigation  capability  than  any  other  navigation  tool  that  exists.  The  use 
of  GPS  as  a  precision  navigation  and  targeting  tool  was  demonstrated  in  the  first 
Gulf  War,  where  the  importance  and  utility  of  GPS  was  proven  [12].  Because  of  the 
great  success  using  GPS,  a  strong  reliance  on  GPS  has  been  developed.  However, 
GPS  is  susceptible  to  periods  of  unavailability,  which  can  be  caused  by  the  terrain, 
environment,  or  interference  from  an  outside  source.  In  these  situations,  a  viable 
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alternative  to  GPS  navigation  has  to  be  implemented. 


This  research  will  use  real  data  to  validate  the  work  found  in  [15]  that  utilized  sim¬ 
ulated  data.  To  do  so,  a  monocular  camera  computer  vision  algorithm  (that  measures 
2-D  normalized  velocity  vectors  that  are  orthogonal  to  the  direction  of  travel)  will  be 
integrated  with  an  IMU,  and  a  range  of  GPS  availabilities,  from  no  satellite  vehicles 
available  to  a  full  GPS  navigation  solution.  The  results  will  show  how  the  monocular 
camera  computer  vision  algorithm  presented  in  this  work,  capable  of  operating  with 
a  monocular  camera  pointing  in  any  direction,  can  help  mitigate  errors  from  a  GPS 
outage,  and  provide  a  better  navigation  solution  than  which  would  be  available  if  the 
system  operated  on  GPS  and  IMU  solutions  alone. 


1.1  Research  Objective 

The  main  objective  for  this  research  was  to  implement  a  monocular  camera  com¬ 
puter  vision  algorithm  that  uses  real  data  to  provide  velocity  error  feedback  to  a 
commercial  grade  IMU.  The  monocular  camera  computer  vision  algorithm  presented 
in  [15],  which  use  features  detected  by  a  monocular  camera  and  rotation  given  by  the 
IMU,  will  be  implemented  in  this  thesis.  In  conjunction  with  the  monocular  camera 
computer  vision  algorithm  and  the  IMU,  a  range  of  GPS  solutions  will  be  employed 
to  test  the  effectiveness  of  aiding  the  system  with  a  monocular  camera.  By  pair¬ 
ing  the  three  sensors,  monocular  camera  computer  vision  algorithm,  IMU,  and  GPS, 
the  usability  of  a  monocular  camera  as  a  supplement  to  GPS  and  how  the  overall 
performance  of  the  navigation  system  can  be  increased  will  be  shown. 
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1.2  Scope 


Because  all  of  the  measurements  used  in  this  thesis  came  from  the  All  Source 
Positioning  Navigation  (ASPN)  test  set,  test  scenarios  were  limited  to  the  data  that 
had  been  previously  recorded.  This  meant  that  additional  runs  for  further  evaluation 
could  not  be  conducted.  It  also  meant  the  lack  of  some  measurements  in  certain 
scenarios  had  to  be  accounted  for  and  removed  when  providing  the  results  of  the 
system. 

1.3  Thesis  Overview 

Chapter  II  of  this  thesis  develops  the  mathematical  background  required  to  ef¬ 
fectively  complete  this  work  along  with  the  math  notation.  Included  in  this  is  the 
camera  model,  feature  detection,  epipolar  geometry,  coordinate  frames  and  transfor¬ 
mations,  the  Kalman  Filter,  and  the  extended  Kalman  filter  (EKF).  An  overview  of 
related  research  is  also  presented. 

Chapter  III  presents  the  methodology  behind  each  sensors  measurements.  The 
IMU  simulation  algorithm  and  the  error  state  model  are  also  presented  here.  Finally, 
all  the  sensors  are  tied  together  through  the  EKF,  which  is  described  in  this  chapter. 

Chapter  IV  highlights  the  results  that  were  obtained  by  the  culmination  of  this 
research.  Each  scenario  will  be  presented  with  an  IMU  only  solution,  GPS  aided, 
computer  vision  aided,  and  both  GPS  and  computer  vision  aided.  Final  results  will 
be  compared  to  each  other  to  show  overall  system  performance  increase  with  the  ad¬ 
dition  of  each  sensor. 

Chapter  V  will  provide  a  summary  of  everything  discussed  in  previous  chapters, 
along  with  the  future  work  to  be  considered. 
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II.  Background 


The  information  that  is  outlined  in  this  chapter  is  background  material,  which  is 
required  to  understand  this  thesis. 

2.1  Mathematical  Notation 

The  mathematical  notation  that  will  be  implemented  for  this  thesis  is  laid  out  as 
follows. 

•  Vectors:  Vector’s  will  be  formed  in  column  elements  and  annotated  with  bold 
font  lower  case  letters,  (e.g.  y  or  p).  A  vectors  specific  scalar  elements  will  be 
represented  by  x,,  where  %  defines  the  column  element  number. 

•  Matrix:  Matrices  will  be  annotated  with  bold  font  upper  case  letters,  (e.g.  X 
or  Tr).  Matrix  row  and  column  elements  will  be  annotatd  as  Xy,  where  i  is  the 
row  index  and  j  is  the  column  index. 

•  Scalar:  Scalars  will  be  annotated  with  a  non-bold  lower  or  uppercase  letters, 
(e.g.  f  or  Z) 

•  Reference  Frames:  Specific  reference  frames  will  be  annotated  with  an  upper 
case  letter  followed  by  a  lower  case  superscript,  (e.g.  Fn  or  Gb) 

•  Direction  Cosine  Matrix:  DCMs  will  dictate  the  state  in  which  the  frame 
is  being  transformed  from  (subscript)  to  the  frame  that  it  is  being  transformed 
to  (superscript)  (e.g.  Cbs  or  C£). 

2.2  Camera  Properties 

In  this  section,  information  pertaining  to  the  camera  model,  feature  detection, 
and  epipolar  constraints  between  images  will  be  conceptualized. 
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Thin  Lens. 


The  imagery  information  that  is  given  by  the  monocular  camera  is  a  feature’s 
re-projection  from  a  real  world  location  through  a  lens  onto  a  camera  image.  The 
scene’s  projection  on  to  the  camera’s  image  (in  this  instance)  is  pictorially  shown  by 
the  thin  lens  camera  model  (Figure  1)  [7]. 


Figure  1.  Thin  Lens  Camera  Model  [7] 


The  projection  of  the  feature  p  onto  the  camera  image,  in  the  thin  lens  camera 
model  (Figure  1),  is  determined  by  the  feature’s  relative  position  to  the  camera  and 
the  optical  lens  of  the  camera.  The  function  of  the  thin  lens  is  characterized  by  two 
properties  [7].  The  first  property  is  that  all  rays  entering  the  aperture  parallel  to  the 
optical  axis  intersect  on  the  optical  axis  at  a  distance  f  from  the  optical  center.  The 
second  property  is  that  all  rays  through  the  center  of  the  axis  are  undeflected.  The 
values  that  are  obtained  from  the  thin  lens  camera  model  are  derived  by  using  the 
fundamental  equation  of  the  thin  lens  [7] : 

1  1  _  1 
z  +  ~z~! 

where  Z  is  the  distance  from  the  lens  to  the  object,  z  is  the  distance  from  the  lens 
to  the  image  plane,  and  /  is  the  focal  length.  If  the  aperture  of  the  lens  is  reduced 
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to  zero,  then  the  rays  are  forced  to  enter  the  optical  lens  at  the  center  of  the  lens. 
From  the  second  property  of  the  thin  lens,  it  is  established  that  those  rays  which  pass 
through  the  center  of  the  optical  lens  are  undeflected.  Similar  geometric  triangles 
that  were  seen  in  the  thin  lens  model  earlier  can  now  be  seen  in  the  pin  hole  camera 
model  (Figure  2). 


Figure  2.  Pinhole  Camera  Model 


From  the  pinhole  camera  model,  the  coordinates  of  the  feature  p  =  [ X ,  Y,  Z]T  can  be 
related  to  its  x  image  coordinates  by  the  prospective  projection  [7] : 

x  =  -f^i  y  =  -A ^  (2) 

The  location  of  x  on  the  image  plane  is  inverted  from  it  original  position,  which 
creates  a  negative  sign  in  the  prospective  projection  equation.  To  solve  the  problem 
of  the  reverse  sign,  the  frontal  pinhole  model  will  be  used  (Figure  3). 


Figure  3.  Frontal  Pinhole  Camera  Model 
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From  the  frontal  pinhole  camera  model,  which  will  be  used  in  this  research,  the 
coordinates  of  feature  p  can  now  be  related  to  its  x  image  coordinates  by 

fX  fY 

®  =  /^>  y  =  f-g  (3) 

From  the  image  coordinates  (x,  y),  the  actual  pixel  coordinates  of  the  feature  can 
be  determined.  The  translation  from  the  origin  to  the  pixel  coordinates  is  shown  in 
Figure  4. 


*Y 


S 


X 


Figure  4.  Transformation  from  normalized  coordinates  to  coordinates  in  pixels. 


The  ratio  of  the  pixels  to  the  frames  width  and  height  are  given  by  Sx  and  Sy . 
The  origin  of  the  pixel  coordinates,  for  this  thesis,  will  be  defined  as  the  upper  left 
corner  of  the  image  plane.  The  translation  from  the  original  reference  frame  to  the 
top  left  corner  is  given  by: 


x'  =  xs  +  ox 


(4) 


y'  =  Vs  +  Oy 

where  ox  and  oy  are  the  coordinates  (in  pixels)  of  the  principle  point  to  the  image 
reference  frame,  and  xs  and  ys  are  the  number  of  pixels  away  from  the  origin  in 
horizontal  and  vertical  axes  [7].  The  homogeneous  solution  for  the  pixel  coordinates 
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can  be  achieved  by: 


x' 

0 

Ox 

X 

x'  = 

y' 

= 

0 

Sy 

Oy 

y 

l 

0 

0 

1 

l 

Feature  Detection. 


(5) 


One  of  the  key  aspects  of  using  a  computer  vision  algorithm  is  the  ability  to 
perform  feature  detection  of  objects  in  an  image.  Feature  detection  that  was  invariant 
to  scale  and  rotation  was  performed  using  the  method  detailed  in  [6].  This  section 
will  present  the  Scale  Invariant  Feature  Transform  (SIFT)  algorithm  that  will  be  used 
in  this  research. 

The  SIFT  algorithm  defined  in  [6]  works  off  of  four  key  elements. 


1.  Scale-space  peak  selection:  Features  are  selected  based  off  of  their  invariance 
to  scale  by  by  using  the  difference-of-Gaussian  function. 

2.  Keypoint  localization:  A  fit  of  nearby  data  for  location,  edge  response,  and 
peak  magnitude  is  performed. 

3.  Orientation  assignment:  Orientations  are  assigned  to  each  keypoint  location 
based  on  the  local  image  properties.  This  enables  the  descriptors  to  be  rotation 
invariant. 

4.  Keypoint  descriptor:  A  weight  is  assigned  to  local  image  gradients  at  the  se¬ 
lected  scale  in  the  region  around  the  detected  keypoints,  and  the  set  of  weighted 
gradients  are  given  as  a  descriptor  associated  with  the  keypoint. 

From  the  keypoints  that  have  been  determined  by  the  previous  elements,  the  features 
are  then  matched  to  features  in  a  corresponding  image.  These  features  are  matched 
by  a  Best-Bin-First  (BBF)  algorithm.  This  defines  a  feature  point  that  is  matched 


based  off  of  its  first  closest  neighbor  distance  to  its  second  closest  neighbor.  The  value 
assigned  in  this  distance  ratio  is  determined  by  the  user  of  the  algorithm,  and  can 
be  set  very  high  or  low  depending  on  the  validity  of  the  matches  required.  Setting 
a  higher  value  can  return  features  that  are  not  matches,  while  setting  a  too  low  of 
a  number  can  provide  for  an  over  determined  system  that  rejects  actual  matches.  A 
more  defined  explanation  of  the  SIFT  algorithm  can  be  found  in  [6]. 

Epipolar  Geometry. 

Given  a  feature  that  appears  in  two  separate  images,  from  the  same  monocular 
camera,  the  rotation  and  translation  of  the  camera  can  be  determined.  This  rotation 
and  translation  can  then  be  used  to  solve  for  rotation  and  translation  of  a  vehicle,  if 
the  camera  was  rigidly  mounted  to  it.  Solving  for  these  two  factors  can  be  done  by 
using  the  epipolar  constraints  of  the  system  [4],  If  it  is  known  that  the  feature  itself 
has  not  moved,  it  can  be  implied  that  the  only  thing  that  has  moved  is  the  camera. 
The  shared  feature  between  two  images  can  be  seen  in  Figure  5. 


Figure  5.  X  is  the  feature  in  the  real  world,  with  no  known  location,  x  and  x'  is  the 
representative  feature  on  each  of  the  camera  images.  Or  and  Or  are  the  origins  of  the 
two  separate  images  in  the  navigation  frame,  e  and  e’  are  the  epipoles  of  the  images. 

The  epipolar  constraints  are  in  turn  used  to  determine  the  essential  matrix.  The 
essential  matrix,  is  a  function  of  the  rotation  and  translation,  which  satisfies  the 
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constraints 


x'Ex  =  0  (6) 

where  E  is  the  essential  matrix  of  the  system.  To  determine  the  essential  matrix 
from  images,  where  the  origin  and  the  feature  position  are  not  known,  the  eight  point 
algorithm  can  be  used  [7].  Using  at  least  eight  different  features  in  two  separate 
camera  images,  eight  different  linearly  independent  Kronecker  product  [7]  vectors  can 
be  determined: 


a=x  0  x7 

a  =  \xxx'2,  xry'.2,  xxz'2)  ypx^  yxy'2,  yxz'2,  zxx'2,  zxy'2,  zxz'<^  (7) 

X=[a1,a2,...,a1T 

where  \  is  the  vector  of  Kronecker  products  given  by  each  matched  features  between 
the  two  images. 

When  no  outside  noise  sources  are  affecting  the  system  it  can  be  shown  that 


XES  =  0  (8) 

where  Es  is  a  stacked  vector  that  is  equal  to  the  eigenvector  that  corresponds  to  the 
smallest  eigenvalue  associated  with  xTX  [7]  and  is  given  as 

Es  =  [en  )  ©12)  ©13)  e21,  ©22,  ©23)  e31>  e32,  G33]T  (9) 

The  values  found  for  Es  can  then  be  unstacked  into  a  3x3  matrix  that  gives  an 
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estimate  of  the  essential  matrix  Eest  as 


en 

ei2 

e13 

e2i 

e22 

^23 

e31 

e32 

e33 

(10) 


By  preforming  a  singular  value  decomposition  on  the  essential  matrix  estimate,  the 
orthogonal  matrices  U  and  V  can  be  obtained  along  with  the  diagonal  matrix  S.  For 
the  essential  matrix  to  be  considered  valid,  its  determinant  must  be  zero  and  its  two 
non-zero  singular  values  must  be  equal  [16].  To  conform  to  the  requirements  of  the 
valid  essential  matrix,  the  diagonal  matrix  is  set  manually  to  the  following  values 


S' = 


1  0  0 
0  1  0 
0  0  0 


the  projected  essential  matrix  can  be  determined  [16]: 


(11) 


E  =  US'VT 


(12) 


2.3  Attitude  Representation 

This  section  depicts  three  different  mathematical  representations  that  are  used  to 
define  the  attitude  of  a  body.  The  three  representations  are  the  direction  cosines, 
Euler  angles,  and  quaternions  [20] ,  and  they  will  be  discussed  in  further  detail  in  this 
section. 


11 


Direction  Cosine. 


The  direction  cosine  matrix  (DCM)  is  a  3x3  matrix,  where  each  row  and  column 
represents  a  unit  vector,  and  the  entire  DCM  is  used  to  rotate  one  frame  of  reference 
into  another  frame  of  reference  [20]. 

DCM  Vector  Transformation. 

The  DCM,  in  this  thesis,  will  be  denoted  by  the  symbol  convention  of  Cb.  Where 
the  components  of  Cbs  are  given  as: 

Cll  C\2  C13 

C21  C22  C23 

C31  C32  C33 

The  elements  in  the  respective  row  and  column  represents  the  cosine  angle  between 
the  reference  frame,  in  this  case  the  sensor  frame  and  the  body  frame.  Given  a  vector 
quantity  in  the  sensor  frame  ns,  the  vector  can  be  transformed  into  the  body  frame 
by  multiplying  it  by  the  DCM  as  shown  by: 

nb  =  Cbsns  (14) 


One  of  the  properties  of  the  DCM,  that  will  be  used  in  this  research,  is  the  transposing 
or  inverting  of  the  original  DCM,  allowing  for  transformation  from  the  body  frame 
to  the  sensor  frame: 

Cl  =  (C‘)T  (15) 
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Euler  Angles. 


By  performing  a  set  of  three  successive  rotations  about  the  three  axes,  a  transfor¬ 
mation  from  one  reference  frame  to  the  next  can  be  obtained.  Euler  angles  used  for 
the  transformation  are  represented  by  the  rotation  about  their  respective  axis  roll(0) 
about  the  x-axis,  pitch($)  about  the  y-axis,  and  yaw  (VO  about  the  z-axis. 


Euler  Angle  Vector  Transformation. 


By  using  the  rotation  about  the  perspective  axis  three  separate  DCMs  are  defined 
as  seen  below: 

r 

cosip  simp  0 

Ci  =  |  —simp  cosip  0  (17) 

0  0  1 


c2  = 


cosO  0  —sinO 
0  1  0 
sinO  0  cosO 


(18) 


1  0  0 

Cs  =  1 0  cos(p  sin<p  (19) 

0  —sirup  coscp 

where  Ci  is  the  rotation  around  the  z  axis,  C2  is  the  rotation  about  the  y  axis, 
and  C3  is  the  rotation  about  the  x  axis.  By  combining  the  three  separate  DCMs  a 
transformation  matrix,  capable  of  transforming  from  the  sensor  to  the  body  axes,  can 
be  obtained: 

=  CgCaCi  (20) 
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Quaternions. 


The  final  method  that  will  be  discussed  in  this  thesis,  used  to  transformed  one 
frame  into  another,  is  the  quaternion.  By  using  a  single  rotation  about  a  vector, 
defined  in  the  reference  frame,  the  quaternion  attitude  representation  can  be  used 
to  transform  one  co-ordinate  frame  to  another  [20].  This  method  is  preferred  when 
dealing  with  a  single  rotation  about  one  axis  and  will  be  used  directly  when  simulating 
the  IMU  measurements  later  on. 

Quaternion  Vector  Transformation. 

The  quaternion  is  a  four  element  vector  that  is  a  translation  of  an  angular  move¬ 
ment  from  one  co-ordinate  frame  to  another  frame.  To  establish  the  quaternions 
needed  to  rotate  from  one  frame  to  another,  the  angle  rotation  values  about  each  axis 
must  be  used.  The  angle  vector  yn  is  expressed  as  three  separate  components  of  the 
angel  vector  by  ,  f-iy ,  and  n  and  has  a  magnitude  //.  The  quaternion  values  can 
then  be  obtained  by  [20]: 

a  cos(fi/2) 

b  ux/ Lisin(u/2) 

q  =  =  ^ ’  (21) 

c  fjiy/ fisin(n/ 2) 

d  Hz/ fj,sin(fj,/2) 

The  quaternions  values  obtained  can  then  be  used  to  form  a  direction  cosine  matrix 
that  can  be  used  to  rotate  from  one  frame  to  another: 

(a2  +  b2  —  c2  —  d2)  2  (be  —  ad)  2  (bd  +  ac ) 

C  =  2  (be  +  ad)  (a2  —  b2  +  c2  —  d2)  2  (cd  —  ab)  (22) 

2  (bd  —  ac)  2  (cd  +  ab)  ( a 2  —  b2  —  c2  +  d2) 
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Just  like  the  DCM  seen  earlier  in  this  chapter,  this  C  value  can  be  used  to  rotate 
from  one  frame  to  another: 

ns  =  Cnfc  (23) 

where  C  is  used  to  rotate  n  from  the  body  frame  to  the  sensor  frame. 


2.4  Reference  System 


This  section  defines  the  reference  system  that  will  be  used  through  out  this  thesis. 
It  is  known  that  the  earth  is  not  shaped  as  an  exact  sphere.  Instead,  it  is  shaped 
more  like  an  ellipsoid  with  a  slight  flattening  occurring  at  the  poles.  To  correct  for 
the  flattening  of  the  earth  at  the  poles,  the  World  Geodetic  System  1984  (WGS  84) 
coordinate  system  serves  as  a  geometric  shape,  represented  as  an  ellipsoid,  where 
the  geometric  mass  has  a  fixed  point  at  the  center  of  the  mass  with  a  x,  y  and  z 
axes  [2],  The  location  of  a  point  p  on  an  ellipsoid  is  defined  by  its  geodetic  latitude, 
longitude,  and  height.  Geodetic  latitude  at  a  point  on  the  surface  of  the  earth  is  the 
angle  between  the  equatorial  plane  and  a  line  normal  to  the  reference  ellipsoid,  which 
passes  through  the  point  [20].  Meridian  radius  of  curvature  and  transverse  radius  of 
curvature  for  point  p,  that  has  geodetic  latitude  and  longitude,  can  be  found  using 


[20] 


_  R(1  -e2) 

(1  —  e2sm2L)3/2 


Re  — 


R 

(1  —  e2sm2L)1/2 


(24) 

(25) 


where  Rn  is  the  meridian  radius  of  curvature,  Re  is  the  transverse  radius  of  the 
curvature,  e  is  the  major  eccentricity  of  the  ellipsoid,  R  is  the  length  of  the  semi¬ 
major  axis,  and  L  is  the  geodetic  latitude  [20].  By  using  the  values  determined  by  the 
meridian  radius  of  curvature  and  transverse  radius  of  curvature,  point  p’s  coordinates 
can  be  converted  to  a  local  level  navigational  frame. 


15 


2.5  Reference  Frames 


In  order  to  avoid  errors  while  navigating,  a  system  needs  to  be  defined  in  the 
correct  reference  frame.  The  reference  frames  that  will  be  used  in  this  thesis,  as 
described  in  [20],  will  be  discussed  in  this  section. 

Inertial  Frame. 

The  inertial  frame  (i-frame)  has  an  origin  that  is  centered  at  the  Earth’s  center 
and  the  axes  are  fixed  with  respect  to  the  stars.  The  axes  are  defined  by  xt,  yt  )  and  zit 
as  seen  in  Figure  6.  In  this  frame  of  reference,  the  z,  axis  coincides  with  the  Earth’s 
polar  axis  that  is  assumed  to  be  invariant  in  direction. 

Earth  Frame. 

The  Earth  frame  (e-frame)  has  an  origin  that  is  centered  at  the  center  of  the 
Earth,  and  the  axes  are  fixed  to  the  Earth.  The  axes  are  defined  by  xe,  ye,  and  ze  in 
Figure  6.  Just  like  the  inertial  frame,  the  ze  axis  coincides  with  the  the  Earth’s  polar 
axis.  The  xe  axis  lies  along  the  intersection  of  the  Greenwhich  meridian  plane  and 
the  Earth’s  equatorial  plane.  The  Earth  frame  rotates  about  the  zt  axis,  with  respect 
to  the  inertial  frame,  at  a  rate  of  12. 

Navigation  Frame. 

The  navigation  frame  (n-frame)  is  a  local  geographical  frame  that  has  its  origin 
at  the  location  of  the  navigational  system.  The  position  of  the  navigation  frame  p 
has  axes  that  are  aligned  in  the  directions  of  north,  east  and  local  vertical  (down)  as 
shown  in  Figure  6.  Rotation  rate  and  velocity  in  the  north,  east  and  down  direction  of 
the  navigation  frame  with  respect  to  the  earth  frame,  co":n,  is  dictated  by  the  location 
of  the  position  p  on  the  Earth.  Rotation  rate  in  the  navigation  frame  with  respect  to 
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the  earth’s  frame  is  measured  as: 


u> 


n 

en 


Ve  -Vn 
RE~\~h  R]\f-\-h 


T 


—  VEtanL 

Rn  ~\~h 


(26) 


where  Rn  and  Re  are  the  meridian  radius  of  curvature  and  transverse  radius  of 
curvature  that  were  defined  in  (2.4).  The  measured  velocity  of  the  system  in  the 
navigation  frame  are  detailed  as  Ve  and  Vn-  The  skew  symmetric  form,  of  the 
rotation  rate  is  given  as 


0 

VEtanL 

—  Vn 

Rn  ~\~h 

RN+h 

—  VEtanL 

0 

~VE 

R  ]\f  -\-h 

Re+L 

Vn 

VE 

0 

R  n  +h 

RsLh 

(27) 
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Figure  6.  Earth  ,  Inertial,  and  Navigation  reference  frames  [23] 
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Body  Frame. 


The  body  frame  (b-frame)  is  used  to  describe  the  orientation  of  a  vehicle  with  an 
origin  centered  at  the  navigation  frame.  Figure  7  shows  the  axes  in  which  the  body 
rotates  around  to  give  the  roll,  pitch,  and  yaw  of  the  body. 


Figure  7.  Body  Frame  of  Reference 


2.6  Kalman  Filtering 

This  section  will  provide  the  back  ground  of  the  Kalman  filter  and  the  extended 
Kalman  filter.  These  filters  will  be  used  through  out  this  thesis  to  estimate  states  of 
a  given  system. 

System  Dynamics. 

The  Kalman  filter  is  an  optimal  recursive  data  processing  algorithm [9].  Back¬ 
ground  information  pertaining  to  the  Kalman  filter  has  been  defined  many  times 
before,  and  instead  of  establishing  a  new  layout  for  the  formulas,  this  thesis  will  draw 
from  formulas  presented  in  [23].  By  knowing  the  systems  dynamics,  initial  conditions, 
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and  all  statistical  information  describing  the  system,  the  Kalman  filter  can  give  the 
optimal  estimate  of  a  linear  system.  Before  introducing  the  Kalman  filter,  a  stochas¬ 
tic  linear  system  model  needs  to  be  defined. 

The  stochastic  linear  system  model  is  defined  as  a  differential  equation: 

x(t)  =  F  (t)x(t)  +  B(£)u(£)  +  G(f)w(f)  (28) 

where  F  is  the  homogeneous  system  dynamics,  x(£)  is  the  state  vector,  B  is  the  input 
matrix,  u  is  the  input  vector,  G  is  the  noise  matrix,  and  w (t)  is  the  vector  of  Gaussian 
white  noise.  The  noise  vector  has  process  noise  strength  Q  (t)  where: 

E{w(t)wT(t  +  r)}  =  Q(/)h(r)  (29) 

where  8(t)  is  the  Dirac  delta  function.  As  stated  earlier  in  this  section,  one  of  the  key 
components  needed  by  the  Kalman  filter  to  characterize  the  system,  is  the  system’s 
statistics.  The  system’s  mean  (mx(f))  and  covariance  (P;CX(t))  can  be  determined  by: 

m  x(t)  =  E{x(t)xT(t)j  (30) 

P xx (t)  =  E{x(t)xT(t)}  -  mx(t)inx{t)  (31) 

where  £7{}  is  the  expectation  operator. 

The  previous  equations  in  this  section  characterize  the  system  in  continuous  time. 
The  Kalman  filter  deals  with  measurements  as  discrete  inputs.  The  current  time 
will  be  denoted  by  A,  and  the  previous  time  step  will  be  denoted  by  4-i  •  The 
discrete-time  model  will  then  have  the  form  of: 

x(4)  =  #(4, 4_i)x(tfc_i)  +  B(4)u(4)  +  G(4)w(4)  (32) 
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where  $  is  the  state  transition  matrix,  x  is  the  vector  of  the  discrete  states,  B  is 
the  discrete  input  matrix,  u  is  the  vector  of  discrete  inputs,  G  is  the  discrete  noise 
matrix,  and  w  is  the  discrete  noise  strength.  The  state  transition  matrix  can  be 
calculated  as: 

4>(4,tt-1)  =  eF<‘‘)a'  (33) 

where  At  is  the  change  in  time. 

The  discretized  noise  power  matrix  Q d(tk)  will  be  determined  by  using  the  Van 
Loan  method  [22],  The  three  step  Van  Loan  method  is  given  as 

— F (4)  G(4)Q(4)G(4)T 
0  FT 

-  ^(4)"1  Qd(4)  _  Bll  B12 
0  $Td  B21  B22 

*&)  =  mk)T)T 

Q  d(tk)  =  B22B12  =  $(4)$(4)_1Q<*(4) 

Discrete  measurements  for  a  linear  system  are  modeled  as: 

Z  (4)  =  H  (4)x(4)  +  v(4)  (38) 

where  H(4)  is  the  observation  matrix,  x(4)  is  the  state  vector  at  that  time,  and 
v(4)  is  the  discretized  noise  vector.  Similarly,  discrete  measurements  for  a  non  linear 
system  will  be  modeled  as: 

z  (4)  =  h(x(4),  4)  +  v(4)  (39) 


(35) 

(36) 

(37) 


20 


where  h  is  the  non  linear  function  of  x(4)  at  time  t/;. 


Kalman  Filter  Equations. 

This  section  will  define  the  equations  that  are  associated  with  the  linear  Kalman 
filter.  The  Kalman  filter  has  two  modes  that  it  operates  in;  it  is  either  propagating  a 
measurement  forward  or  updating  the  system  with  a  new  measurement.  During  the 
propagation  state,  the  system  is  using  the  state  transition  matrix  <&(£*,)  to  propagate 
the  states  estimate  forward  in  time.  Not  only  are  the  states  propagated  forward,  but 
the  systems  error  covariance  matrix  is  propagated  through  time  as  well.  The  Kalman 
filter  propagation  model  is  shown  here: 

*(#)  =  *(<»)*(<*)  (40) 

p  M)  =  *Pfe-)«>T  +  Q«(«  (41) 

where  x  is  an  estimate  of  the  state’s  mean,  P  is  an  estimate  of  the  covariance  of  the 
system,  denotes  the  estimate  of  the  covariance  and  mean  at  the  time  immediately 
prior  to  tk  and  denotes  the  estimate  of  the  mean  and  covariance  after  the  update. 
When  estimates  of  the  covariance  and  mean  are  not  available: 


x(tp  =x(tt) 

(42) 

p  W)  =  p  (*~t) 

(43) 

When  a  measurement,  z (t),  from  the  system  becomes  available,  an 

values  are  performed.  The  update  equation  for  the  Kalman  filter  is: 

update  of  the 

*(# )  =  x(ifc)  +  K(4)(z(4)  -  H(4)x(tAT)) 

(44) 
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p<(t+)  =  (i  -  K(t*)H(«*))p(tn 


(45) 


where  I  is  an  identity  matrix  and  is  the  Kalman  gain.  The  Kalman  gain  given  by 

K(4)  =  P(^)H(4)t(H(4)P(V)H(4)t  +  R(4))_1  (46) 

optimally  weights  the  measurements  that  are  received.  The  R  value  in  the  Kalman 
gain  equation  is  the  covariance  of  the  measurements  and  is  defined  by: 

R(4)  =  E[(v(tk))(v(tk)T]  (47) 

The  Kalman  filter,  if  modeled  correctly  (i.e. ,  errors  in  the  filter  to  do  not  create  a 
non  determinant  solution)  and  provided  with  new  state  estimates,  will  continually 
cycle  between  propagate  and  update.  If  the  system  is  not  modeled  correctly,  the 
Kalman  filter  eventually  collapses  because  the  state  transition  matrix  will  become 
non-invertible.  The  best  place  to  check  if  the  filter  is  operating  correctly  would  be 
the  output  of  the  residuals.  Residuals  (r)  of  the  system  can  be  calculated  by: 

r  =  z (4)  -  H(4)x(tfc )  (48) 

Residual  values  will  usually  increase  exponentially  if  the  Kalman  filter  is  modeled 
incorrectly. 

Extended  Kalman  Filter. 

In  the  case  of  a  measurement  or  system  that  is  non-linear,  the  normal  Kalman 
filter  can  not  be  implemented  and  the  use  of  the  extended  Kalman  filter  (EKF)  is 
required.  This  section  will  discuss  the  background  behind  the  EKF  [8].  Just  like  the 
Kalman  filter,  the  EKF  has  two  modes,  update  and  propagate.  The  EKF  is  described 
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by  the  dynamic  model: 


x(4)  =  f[x(4),  u(ifc),  4]  +  G(4)w(4)  (49) 

where  the  x(4)  value  is  the  state  vector,  u (4)  is  a  vector  of  inputs  ,  and  w (4)  is  the 
zero-mean  white  Gaussian  noise  for  the  system.  The  measurement  equation  for  the 
EKF  is  similar  to  that  of  the  Kalman  filter  with  a  small  change  of  it  being  able  to 
handle  the  nonlinear  models.  The  measurement  equation  for  this  filter  is: 

z  (4)  =  h[x(4),  4]  +  v(4)  (50) 


A  large  part  of  the  EKF  is  to  be  able  to  linearize  around  the  measurements  that  are 
received.  This  action  can  be  completed  by  taking  the  Jacobian  of  both  the  dynamics 
equation  and  the  measurement  equation.  The  Jacobian  for  the  two  equations  are 
derived  as: 


F[4;xn(4)]  = 
H[4;xn(4)] 


a  Jf[x,u(4),4] 


5x 

Jh[x,4] 


X=xn  ( tk ) 


Sx 


(51) 


(52) 


lx=xn  ( tk ) 

The  EKF  states  and  covariances  are  propagated  in  the  same  manner  as  found  in 
Equations  (41)  and  (  40).  The  updates  for  the  EKF  utilize  both  the  linear  and  non 
linear  equations  and  can  be  seen  here: 


*(# )  =  x(4 )  +  K(4){z(4)  -  h[x(4 ),  4]}  (53) 


p (ti)  =  P(ffc  )  -  K(4)H[4;  x(4-)]P(4")  (54) 
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The  Kalman  gain  equation  for  the  EKF  is  similar  to  that  of  the  Kalman  filter  and  is 
calculated  as: 


K(U)  =  P(i-)HT[tj;  x(t-)]{H[tj;  x(t-)]P(i-)HT[tj;  x(t-)]  +  Rff,)}-1  (55) 

2.7  Previous  Research 

Implementation  of  a  paired  visual  system  and  commercial  IMU  for  a  more  robust 
navigation  system  in  an  urban  environment  is  not  a  new  concept.  For  this  research, 
only  systems  that  used  monocular  imagery  along  with  IMU  integration  were  reviewed. 
The  four  most  related  research  areas  that  will  be  evaluated  in  this  section  are  [15], 
[13], [17],  and  [10]. 

Simulation  Platform  for  Vision  Aided  Inertial  Navigation [15]. 

Png’s  research  is  the  predecessor  to  the  research  that  is  developed  in  this  docu¬ 
ment.  Instead  of  implementing  the  vision  aided  navigation  in  a  real  world  environ¬ 
ment,  his  research  simulated  all  of  the  sensors  that  were  used.  By  simulating  the 
data,  normal  anomalies  that  are  inherent  with  IMU’s  and  GPS  could  be  reduced  or 
even  eliminated.  The  simulated  system  was  able  to  apply  errors  in  a  specific  area 
and  then  evaluate  the  results  knowing  that  nothing  else  was  affecting  the  system.  A 
relationship  between  the  measurements  provided  with  different  qualities  of  cameras 
was  just  one  of  the  scenarios  that  was  able  to  be  tested.  Another  factor  that  was 
tested,  which  was  difficult  to  achieve  in  this  research,  was  a  satellite  vehicle  (SV)  that 
was  consistently  insight  for  the  entire  duration  of  the  scenario  and  was  not  affected  by 
outside  interference  (i.e.,  multi-path,  building  obstruction,  or  atmospheric  effects). 

Png’s  research  further  developed  the  value  of  having  a  visual  odometry  system 
to  aid  the  system  in  a  GPS  degraded  environment  [15].  Conclusions  drawn  from  his 
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research,  that  will  now  be  verified  in  this  research,  is  that  the  system  sees  a  boost  in 
overall  performance  when  the  system  is  aided  with  vision  and  GPS. 

Vision- Aided  Inertial  Navigation  using  Planar  Terrain  Features  [13]. 

Panahandeh/Jansson  discuss  the  implementation  of  a  ground  facing  monocular 
camera  attached  to  an  IMU  that  detects  ground  plane  features.  The  strength  behind 
this  system  is  its  ability  to  give  observability  to  the  scale  of  a  feature  while  only  using 
a  single  camera. 

By  locking  the  camera  in  a  downward  facing  pose,  scale  estimation  of  the  3-D 
camera  translation  can  be  solved.  Features  that  are  detected  can  all  be  thought  of 
as  having  the  same  distance  from  the  camera.  However,  this  is  not  the  case  when  a 
camera  is  pointed  forward  or  sideways.  A  dynamic  background  like  this  can  lead  to 
measurement  errors  because  of  the  nonlinearity  between  features. 

Real-Time  Monocular  Visual  Odometry  for  On-Road  Vehicles  with  1- 
Point  RANSAC  [17]. 

Scaramuzza/et  al  discuss  the  use  of  nonholonomic  constraints  of  a  wheeled  vehicle, 
that  has  an  imagery  system  with  a  very  fast  frame  rate.  They  also  note  two  separate 
outlier  rejection  algorithms  that  can  be  used  for  feature  rejection.  The  whole  concept 
behind  this  paper  is  the  amount  of  information  that  can  be  used  by  constraining  the 
dynamics  of  the  vehicle  to  the  Ackerman  steering  principle  [18].  A  structure  from 
motion  (SFM)  technique  is  also  used  for  the  outline  of  the  vehicle  estimated  motion. 
This  can  be  done  because  the  frame  rate  of  the  camera  is  so  quick,  and  there  are  no 
gaps  in  feature  detection  between  frames. 

Also  important  to  note  is  the  use  of  the  two  outlier  rejection  methods  of  the  system. 
One  of  them  is  the  single  point  random  sample  consensus  (RANSAC)  algorithm  that 
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cuts  down  on  the  computational  time  for  outlier  rejection.  The  other  is  a  histogram 
voting  technique,  that  uses  the  angle  calculated  to  each  feature  to  provide  a  histogram 
of  angles  that  coincide  with  each  other.  Points  are  then  voted  on  and  then  chosen  by 
the  their  angle  which  is  related  to  most  of  the  other  features. 

Realtime  Implementation  of  Visual- Aided  Inertial  Navigation  using 

Epipolar  Constraints  [10]. 

Norrison/et  al  discuss  the  real  time  implementation  of  a  visual  aided  inertial 
navigation  system  that  is  based  off  of  epiploar  constraints.  While  measurements 
equations  that  are  presented  in  their  work  and  this  thesis  are  going  to  differ,  the  way 
that  they  use  the  IMU  to  perceive  a  pure  translation  of  the  imagery  system  is  similar. 
They  refer  to  their  method  as  rotation  unwrapping  of  feature  points  (FPs)  based  on 
the  IMU  sensed  rotation  over  the  period  between  images. 

IMU  rotation  measurements  are  considered  to  have  very  low  error  (ldeg)  over 
time  periods  up  to  a  minute  [10].  By  combining  the  rotation,  given  by  the  IMU,  and 
the  pure  translation  of  the  FPs  that  have  been  rotation  unwrapped,  the  fundamental 
matrix  can  be  well  defined.  Having  a  well  defined  Fundamental  matrix  plays  a  large 
part  in  their  outlier  rejection,  which  works  off  the  principle  of: 

x,tFx  =  0  (56) 

where  F  is  the  fundamental  matrix.  Instead  of  defining  the  value  of  rejection  to  be 
zero,  their  research  changes  the  value  to  a  rejection  threshold  value  that  is  defined 
by  the  angle  between  points  and  distance  of  movement.  They  then  incorporate  their 
returned  measurements  as  errors  in  position.  These  measurements  are  calculated  by  a 
bundle  adjustment  of  the  current  position  error  detected  by  FPs  along  with  previously 
determined  camera  measurements  that  hold  the  most  statistical  relevance. 
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Summary. 


This  chapter  developed  the  mathematical  notation  and  specific  equations  that 
will  be  utilized  throughout  this  thesis.  Camera  properties  such  as:  the  thin  lens, 
pinhole  camera  model,  and  projective  equations  were  illustrated.  The  relationship  of 
consistent  camera  images  was  utilized  by  epipolar  geometry  between  features  in  an 
image,  to  determine  the  essential  matrix. 

Attitude  representation  was  also  described  as  a  way  to  move  from  one  frame  of 
reference  to  another  frame  of  reference.  The  reference  frames  discussed  in  this  chapter 
included  the  Earth  Center  Earth  Fixed  (ECEF),  Inertial,  Navigation,  and  the  Body 
frame.  The  specific  attitude  of  the  system  will  be  represented  by  DCMs,  Euler  angles, 
or  quaternions. 

Development  of  the  state  space  model  along  with  the  techniques  behind  state 
and  covariance  estimation  from  the  Kalman  filter  and  EKF  were  also  developed. 
Measurements  that  will  be  supplied  to  the  filter,  will  be  further  developed  in  the  next 
chapter. 

A  summary  of  previously  accomplished  work  in  the  same  area  was  also  conducted. 
These  research  points  show  some  ideas  of  additional  models  that  could  be  added  to 
the  work  presented  in  this  thesis  or  future  research.  Some  of  the  ideas  that  would  be 
pertinent  to  include  in  this  work  could  not  be  accomplished  because  of  the  data  sets 
that  were  provided. 


27 


III.  Methodology 


This  chapter  outlines  the  methods  that  were  used  to  complete  this  thesis.  Illustra¬ 
tions  of  the  information  obtained  from  monocular  camera  computer  vision  algorithm, 
IMU,  and  GPS  will  also  be  shown.  Additionally,  measurement  equations  that  link 
individual  monocular  computer  vision  algorithm,  IMU,  and  GPS  to  the  system  along 
with  system  dynamics  will  be  described.  A  block  diagram  describing  the  loosely 
coupled  system  is  shown  in  Figure  8. 


Figure  8.  Loosely  coupled  feedback  approach  utilized  in  this  work.  The  monocular 
camera  system  provides  Yp  and  Y2  which  are  the  matched  features  detected  in  two 
sequential  images.  GPS  provides  pseudoranges  for  each  satellite  that  is  measured  from 
the  SV  to  the  receiver  on  the  vehicle.  The  INS  provides  estimated  position  (pn)  in  the 
navigation  frame,  estimated  velocity  (vn)  in  the  navigation  frame,  estimated  orientation 
(C^)  of  the  vehicle,  and  gyro  measurement  (A0S)  in  the  IMU’s  frame  of  reference.  The 
Extended  Kalman  filter  provides  errors  for  the  position  (Spn)  in  the  navigation  frame, 
velocity  (£vn)  in  the  navigation  frame,  and  tilt  errors  (-0). 


3.1  Imagery  Information 

This  section  will  define  the  characteristics  of  feature  detection  implemented  in 
this  thesis,  along  with  development  of  the  monocular  camera  computer  vision  algo¬ 
rithm  2-D  zero  velocity  measurements  that  can  be  supplemented  into  the  EKF  for 
measurement  updates. 
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Feature  Detection. 


As  pairs  of  images  are  processed  throughout  this  research,  a  feature  detection  and 
matching  algorithm  is  being  run  to  ensure  that  features  between  two  images  match. 
For  this  particular  research,  the  Scale  Invariant  Feature  Transform  (SIFT)  algorithm, 
defined  in  Section  2.2,  was  used  as  a  tool  for  feature  detection.  Features  detected 
by  a  monocular  camera  at  two  separated  times  are  compared  and  then  accepted  or 
rejected  based  on  the  Mahalanobis  distance  between  the  descriptors.  Users  are  allowed 
to  define  the  distance  ratio,  which  defines  the  minimum  Euclidean  distance  for  the 
descriptor  from  its  nearest  neighbor  to  its  second  closest  neighbor.  Depending  on  the 
value  set  for  the  distance  ratio  one  of  three  things  can  happen:  mismatches  can  be 
accepted  as  matches,  matches  can  be  rejected  because  the  system  is  overdetermined, 
or  correct  matches  can  be  returned.  By  setting  the  distance  ratio  of  the  system  to 
be  .35,  the  features  that  are  returned  as  matches  seem  to  have  very  few  mismatches. 
Images  are  undistorted  and  then  run  through  the  SIFT  algorithm. 

After  using  SIFT,  a  pixel  location  of  matching  features  between  the  two  images 
is  returned.  By  using  the  intrinsic  camera  calibration  matrix,  feature  pixel  locations 
are  then  converted  to  projected  x  and  y  locations  as  seen: 


A 


Fx  0  Ox 

0  Fy  Oy 


0  0  1 


(57) 


Sproj  A 


-1, 


3 pix 


(58) 


where  A  is  the  camera  calibration  matrix,  Fx  and  Fy  are  determined  by  the  focal 
length  and  pixel  count  for  both  the  height  and  width  of  the  image,  Ox  and  Oy  are 
the  intersection  points  of  the  optical  axis  with  the  image  plane,  and  sproj  and  s[nx  are 
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the  vectors  of  the  camera  frame  and  the  projected  frame.  By  way  of  demonstration, 
Figure  9  shows  a  pair  of  images  with  a  line  connecting  the  features  that  have  been 
matched  by  the  SIFT  algorithm. 
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Figure  9.  Sequence  of  two  images  side-by-side  with  matching  features  connected  by  a 
green  line. 

Outlier  Rejection. 

Due  to  the  possibility  of  incorrect  features  being  matched  between  subsequent 
images,  a  way  of  rejecting  points  had  to  be  determined.  There  are  several  different 
ways  that  outliers  can  be  rejected  from  the  system.  One  way  of  removing  outliers 
is  to  use  a  histogram  of  closely  related  angular  values  that  relate  the  features  in  the 
first  image  to  the  features  in  the  second  image,  and  to  only  keep  those  points  that  fall 
into  the  most  heavily  populated  bins  [17].  This  method  is  used  to  detect  the  amount 
of  movement  by  its  corresponding  angular  movement  in  the  image.  Another  way  of 
rejecting  outliers  is  to  use  the  epipolar  constraints  of  the  system  and  use  the  calculated 


30 


fundamental  matrix  to  reject  points  [10].  The  fundamental  matrix  could  be  calculated 
one  of  two  ways:  with  rotation  and  translation  measurements  given  by  the  IMU  or 
by  using  the  matched  features  in  the  image.  For  this  thesis,  a  best  fit  fundamental 
matrix  from  image  features  was  used  for  outlier  rejection.  A  RANSAC  [3]  algorithm, 
which  calculated  the  best  fit  fundamental  matrix,  was  implemented  and  used  during 
the  course  of  this  entire  work.  In  addition  to  setting  a  low  distance  ratio  when 
performing  SIFT  on  images,  the  use  of  RANSAC  was  utilized  to  reject  ten  percent  of 
the  points  that  were  run  through  the  algorithm.  A  ten  percent  point  rejection  by  the 
RANSAC  algorithm  was  accomplished  by  setting  the  rejection  threshold  very  high, 
and  then  through  an  iterative  process,  the  threshold  was  decreased  until  no  more 
than  ninety  percent  of  the  matched  features  remained.  Using  the  same  two  images 
that  were  seen  in  Figure  9,  the  features  that  have  now  been  rejected  are  connected 
by  a  red  line  and  shown  in  Figure  10. 


Figure  10.  Sequence  of  two  images  side-by-side  with  matching  features  connected  by  a 
green  line  and  RANSAC  rejected  features  connected  by  a  red  line. 
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Camera  Measurements. 


Based  off  of  the  feature’s  projected  location  between  images,  the  values  of  the 
translation  in  the  camera  frame  could  be  obtained.  A  common  usage  of  feature 
points  is  in  an  eight  point  algorithm  [4]  to  determine  the  essential  or  fundamental 
matrix,  as  described  in  chapter  two.  Once  the  essential  matrix,  defined  in  [4],  has 
been  determined,  a  solution  for  the  translation  t  and  rotation  can  be  solved 

for,  so  that  their  values  satisfy  the  equation: 


E  =  c£3£[t]x  (59) 

However,  multiple  solutions  can  be  derived  from  this  equation  and  even  when  a  solu¬ 
tion  is  thought  to  be  correct,  the  slightest  deviation  in  the  system’s  rotation  can  be 
costly  to  the  translation  vector  returned.  Instead,  this  research  looks  at  using  the  ro¬ 
tation  obtained  from  the  IMU,  and  only  solving  for  the  translation  of  the  system  (t). 
IMU’s,  even  at  the  commercial  grade  level,  provide  very  reliable  rotation  measure¬ 
ments  for  short  periods  of  time.  Common  methods  of  IMU  integration  with  images  is 
done  by  using  the  vehicle’s  orientation  at  the  time  of  images  [24],  For  this  research, 
the  rotation,  sensed  by  the  IMU,  of  the  system  over  the  time  period  between  the  im¬ 
ages  is  tracked  and  stored.  Once  the  second  image  is  obtained,  the  rotations  are  then 
integrated  up  to  the  time  of  the  image.  Next,  the  final  rotation  is  translated  from  the 
IMU’s  frame  of  reference  into  the  camera’s  frame  of  reference.  Features  from  the  first 
image  are  then  rotated  into  the  second  images  frame  of  reference  using  the  rotation 
matrix  obtained  from  the  IMU’s  rotation  measurements.  By  rotating  features  from 
one  frame  into  another,  the  only  disparity  left  from  the  feature  would  be  contributed 
to  the  translation  of  the  vehicle.  Finally,  a  matrix  of  translation  values  is  obtained 
that  relates  all  of  the  features  to  a  specific  translation  sensed  by  them.  Due  to  lack 
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of  depth  knowledge,  or  distance  of  features  in  the  direction  of  movement,  there  can 
be  an  unobservability  in  the  magnitude  of  the  translation  vector.  To  mitigate  the 
problem  and  correct  to  a  best  fit  translation  measurement,  the  use  of  singular  value 
decomposition  (SVD)  was  used  for  the  set  of  homogeneous  solutions  that  are  found. 
An  outline  of  the  equations  used  to  compute  the  translation  of  the  vehicle,  defined  in 
[19],  can  be  seen  here: 

Y image2  _  Qimage2  *  ~Yima9el  (GO) 


T  = 


Yirnage2  ^  *  yj’ 


image2 


(3)  -  Y\ma9e2(2) 


Y image2^  ^  -y^image2 


(!)  -  Y^e2(3) 


Yima0e2^  ^  Yima9e2(2)  —  Yima9e2  (2)  *  Yima9e2 


(3) 


[U,  S,  V]  =  5FD(T) 


(61) 


(62) 


where  Y\maaei  are  the  set  of  features  located  image  one  in  image  one’s  reference  frame, 
Y irnage2  are  (-jie  se£  ()f  features  located  in  image  two  in  image  two’s  reference  frame, 
Y irnage2  are  )-jie  features  from  image  one  rotated  into  the  second  image’s  reference 
frame,  is  the  DCM  used  to  rotate  features  from  image  one  to  image  two,  T  is 

a  matrix  of  the  sensed  translation  values  for  all  features  between  the  two  images,  U  is 
a  matrix  of  the  left  singular  vectors  of  T,  and  S  is  a  diagonal  matrix  that  represents 
the  singular  values  of  T.  The  left  singular  vector  of  U  associated  with  the  smallest 
singular  value  of  S  is  then  used  as  the  measured  translation  vector  tb  as  sensed  by 
the  monocular  camera  computer  vision  algorithm  in  the  body’s  frame  of  reference. 


Zero  Velocity  Vectors. 

Instead  of  using  the  exact  translation  vector  t6  computed  from  the  monocular 
camera  computer  vision  algorithm,  2-D  zero  velocity  vectors  orthogonal  to  the  direc¬ 
tion  of  travel  will  be  calculated.  Because  the  velocity  vector  t6  has  no  magnitudes 
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associated  with  its  values,  tb  can  only  really  give  a  sense  of  direction  and  not  the 
overall  magnitude  of  movement.  By  calculating  the  2-D  zero  velocity  vectors  and  ap¬ 
plying  them  to  the  velocity  measurements  given  by  the  INS,  errors  in  the  direction  of 
movement  can  be  evaluated  instead  of  the  magnitude  of  movement.  Before  converting 
the  translation  vector  tb  into  the  2-D  zero  velocity  vector,  the  translation  vector  has 
to  be  rotated  into  the  correct  sensor  orientation.  In  this  case,  the  translation  vector, 
predicted  by  the  images,  would  be  rotated  from  the  second  image  camera’s  orientation 
into  the  navigation  frame.  However,  the  translation  vector  obtained  by  the  computer 
vision  algorithm  would  not  be  rotated  into  the  navigation  frame  defined  at  the  time 
of  the  final  image.  The  reason  for  not  rotating  the  translation  into  the  navigation 
frame  at  the  time  of  the  second  image  is  that  the  translation  sensed  by  the  cameras 
is  not  for  a  specific  point  in  time,  but  is  better  depicted  as  translation  sensed  over 
a  period  of  time  between  two  separate  poses.  As  a  result,  the  values  of  the  sensed 
translation  vector  are  instead  rotated  into  the  navigation  frame  of  the  vehicle  at  a 
time  halfway  between  the  two  images.  This  can  be  done  because  the  time  between 
measurements  is  small,  so  there  is  not  much  rotation.  Once  the  translation  vector  tb 
has  been  rotated  into  the  correct  frame,  the  2-D  zero  velocity  vectors  can  be  obtained. 
An  example  of  how  these  orthogonal  vectors  are  obtained  is  represented  here: 


tn  =  C£  *  t6 


(63) 


-L? 


|  n 
— 1 —  2 


=  7V(tn) 


(64) 


where  is  the  DCM  that  rotates  the  translation  from  the  body’s  frame  of  reference 
to  the  navigation’s  frame  of  reference  at  a  time  in  between  the  two  images,  N  is  used 
to  denote  the  operation  of  determining  the  null  space  of  the  translation  vector  tn, 
and  _L”  and  _l_2  are  the  two  orthogonal  vectors  in  the  navigation  frame. 
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3.2  IMU  Measurements 


Due  to  lack  of  data  within  some  of  the  ASPN  scenarios  tested  in  this  research, 
simulated  commercial  grade  IMUs  had  to  be  generated  in  order  to  complete  the  work 
outlined  in  this  thesis.  This  section  will  develop  the  process  in  which  these  IMU 
measurements  were  generated. 

Truth  Data. 

To  create  the  simulated  commercial  grade  IMUs,  a  truth  data  set  was  utilized. 
Truth  data  sets  were  created  with  differential  GPS  and  a  navigation  grade  IMU  during 
the  time  of  the  scenario  runs.  Information  that  was  contained  in  these  data  sets  were 
latitude,  longitude,  altitude,  velocity  in  the  north,  east  and  down  directions,  time  of 
measurements,  roll,  pitch,  and  yaw.  All  of  the  measurements  for  the  vehicle  were 
taken  in  the  navigation  frame.  Measurements  for  the  truth  system  were  generated  at 
10  Hz. 

Data  Interpolation. 

To  turn  the  truth  data  sets  into  measurements  that  resembled  an  IMU,  and  were 
usable  by  the  camera,  the  time  between  measurements  had  to  be  drastically  reduced. 
For  this  research,  IMU  measurements  were  generated  at  100  Hz;  therefore,  this  meant 
that  the  truth  data  first  had  to  be  interpolated  at  100  Hz. 

Interpolation  of  the  data  was  very  straight  forward  for  the  time,  latitude,  longi¬ 
tude,  and  altitude.  When  interpolating  the  roll,  pitch  and  heading,  the  values  had  to 
be  transformed  into  quaternions.  By  doing  so,  a  rotation  about  a  single  axis  could 
be  implemented. 
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Simulated  Measurements. 


From  the  interpolated  data,  IMU  measurements  that  represent  the  true  integrated 
specific  force  and  angular  rate  were  calculated.  Next,  the  measurements  were  cor¬ 
rupted  according  to  an  error  model.  Commercial  grade  IMU  error  values  that  were 
used  can  be  seen  here: 


Table  1.  Commercial  Grade  IMU  Parameters 


^  gyro 

8.7E-3 

GyroTimeConstant 

3600 

Angular RandomWalk  (ARW) 

6.5E-4 

@ accel 

1.96E-1 

AccelTimeConstant 

3600 

Velocity  RandomWalk  (VRW) 

4.3E-3 

From  the  error  values  given,  the  commercial  grade  IMU  could  be  generated  using 
the  integrated  measurement  equations  derived  in  [14] .  These  equations  will  be  used  to 
simulate  the  error  induced  IMU  measurements  from  the  true  system’s  measurements. 

Integrated  Angular  Rate  Gyro  Measurement. 

Integrated  angular  rate  measurements  were  obtained  from: 

A0meas  ^■@true  bA0  +  wA6)  (65) 

where  A 6trUe  is  the  true  change  in  angular  rate  for  the  time  period  since  the  last 
measurement,  bA<?  is  the  time  correlated  bias  (TCB)  error,  and  wA©  is  the  measure¬ 
ment’s  white  Gaussian  noise  error,  b  has  an  initialization  value  that  is  determined 
by  the  system’s  gyro  time  correlated  bias  sigma.  After  initialization,  the  system  bias, 
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modeled  as  a  first-order  Gauss  Markov  (FOGM)  process,  in  which: 


b 


1 

T 


b  +  wb 


(66) 


where  T  is  the  time  constant,  b  is  the  time  correlated  gyro  bias,  and  w*,  is  a  white 
Gaussian  noise.  To  be  useful,  the  system  now  needs  to  be  modeled  in  discrete  time. 
From  [9]  the  state  transition  matrix  for  the  system  is  given  as: 


$(4,4-i)  =  e  {tk  tk~l),T 


(67) 


By  letting  the  transient  values  die  out,  where  ta  approaches  negative  infinity,  the 
discrete  noise  value  can  be  calculated  from  the  variance  as  [9]: 

B[b(i)2]  =  erf  =  ic U(h)T 
CM4)  =  ^ 

where  cr%  is  the  variance  of  the  system.  White  Gaussian  noise  that  is  added  to  the 
system  in  the  discrete  time  is  expressed  as: 


ARW2At 


where  the  ARW  is  a  value  associated  with  the  grade  of  IMU  being  used. 

Integrated  Specific  Force  Measurement. 

Integrated  specific  force  measurements  are  very  similar  to  those  found  for  the  in¬ 
tegrated  angular  rate  gyro  measurements,  with  the  main  difference  coming  from  the 
values  used  to  model  the  noise  measurements.  The  integrated  specific  force  measure- 
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ment  is  modeled  as: 


A"Vmeaa  ^Vtrue  bA„  +  wA„  (70) 

where  Avtrue  is  the  change  in  velocity  over  the  time  period,  bA„  is  the  TCB  error, 
and  wA„  is  the  measurement’s  white  Gaussian  noise  error.  The  values  for  <h  and 
Qd(tk)  are  expressed  in  the  same  manner  as  the  angular  rate  gyro  measurement. 
The  Gaussian  noise  is  even  expressed  in  the  same  way,  except  the  angular  random 
walk  (ARW)  in  now  changed  to  velocity  random  walk  (VRW)  as  seen  here: 

<„  =  VRW2  At  (71) 

where  VRW  is  the  velocity  random  walk  associated  with  the  specific  IMU. 


3.3  INS  Error  Model 


This  section  develops  navigation  error  state  vector  i)x  for  the  inertial  navigation 
system  (INS)  error  models  used  to  relate  position  error  (5pn),  velocity  error  (<5vn),  tilt 
error  (-0),  accelerometer  bias  (afc),  and  gyro  bias  (bfc),  seen  in  Equation  72,  to  their 
respective  states  [23].  Accelerometer  bias  and  gyro  bias  have  already  been  discussed 
in  Section  3.2,  and  will  only  be  implemented  in  the  error  states  in  this  section.  These 
models  will  then  be  used  in  the  system’s  state-space  model  to  propagate  errors  when 
error  measurements  are  not  available. 


(72) 

15x1 
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5x  = 


<5pn 

5vn 

*i> 

a6 

bb 


Attitude  Errors. 


Attitude  errors  are  modeled  as  the  vector  (ip)  that  takes  into  account  the  naviga¬ 
tion  frame  angle  errors  in  the  north,  east  and  down  axes.  This  vector  is  a  right  hand 
system  defined  as: 

Vvi 

^  =  ipe  (73) 

ipd 

where  ipn,  xpe,  and  ipd  are  the  small  angle  errors  in  the  navigation  frame.  From  [23], 
the  following  linearized  angular  error  differential  equation  has  been  derived: 

M>  =  -[(C >«x]V>  _  C£b‘  -  q>J  (74) 

where  (C x  is  the  Earth’s  sidereal  angular  rate  rotated  in  the  navigation  frame 
and  displayed  in  the  skew-symmetric  form,  b6  is  the  gyroscope  measurement,  CJ'  is 
the  DCM  from  the  body  frame  to  the  navigation  frame,  and  w(  is  the  additive  white 
Gaussian  noise  process  of  the  gyro. 

Velocity  Error. 

Velocity  errors  are  modeled  in  the  vector  form  of  hvn  and  provide  velocity  errors 
sensed  in  that  navigation  frame  in  the  north,  east  and  down  directions.  This  is  also 
a  right  hand  coordinate  system  that  looks  like: 

8vn 

5wn  =  hve  (75) 

8vd 
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where  Svn  is  the  IMU’s  velocity  error  in  the  navigation’s  frame  north  direction,  five 
is  the  IMU’s  velocity  error  in  the  navigation’s  frame  east  direction,  and  5vd  is  the 
IMU’s  velocity  error  in  the  navigation’s  frame  down  direction.  The  linear  stochastic 
velocity  error  model  derived  in  [23]  is  given  as: 

«v"  =  c’jGgcejP"  -  2c;n  'ecy  v“  +  (r  x  )«/>  +  c;a"  +  cX  (76) 

where  Gg  is  the  gradient  of  the  gravity  vector  as  described  in  [23] ,  pn  is  the  position 
of  the  vehicle  on  the  navigation  frame,  f 2|e  is  the  rotation  of  the  earth  with  respect 
to  the  inertial  frame  in  a  skew  symmetric  form,  (fn  x )  is  the  skew-symmetric  specific 
force  in  the  navigation  frame,  ab  is  the  accelerometer  bias  in  the  body  frame,  C®  is 
the  DCM  from  the  navigation  frame  to  the  ECEF  frame,  and  w][  is  an  additive  white 
Gaussian  noise  process  of  the  accelerometer  in  the  body  frame. 

From  the  velocity  error,  the  position  error  can  then  be  derived  from  the  kinematic 
relationship  between  the  position  and  velocity 

Spn  =  8vn  (77) 


State  Space  Model. 

This  section  defines  the  state-space  model  that  was  used  for  the  error  models 
provided  in  Section  3.3.  The  state-space  model  used  for  this  research  was 

x(£)  =  F  (t)dx(t)  +  G(£)w(£)  (78) 

where  B  (t)  has  been  omitted  because  there  are  no  inputs  into  the  system. 

The  linear  dynamics  matrix  F  [23],  that  is  a  time- varying  function,  is  given  by 
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(79) 


F  = 


0, 


cneGgc: 
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The  noise  distribution  matrix  G  is  given  as 


G  = 


03^03 

o3 

03 

-V  -3- 

o3 

03 

03 

-Q 

o3 

03 

—  J - 

03^03 

13 
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°, 
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J  15x12 


The  noise  source,  w,  is  defined  by  variables  that  have  already  been  derived  in  this 
thesis,  and  takes  on  the  form  of 


w 


W" 


W 


w 


abias 


w 


bbii 


12x1 


3.4  Measurement  Updates 


(81) 


This  section  defines  the  measurement  updates  that  are  provided  to  the  EKF  de¬ 
scribed  in  Section  2.6.  More  importantly,  this  section  will  define  the  measurements 
updates  that  are  provided  by  the  monocular  camera  computer  vision  algorithm  and 
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GPS  measurements. 


Camera  Measurements  Updates. 

The  2-D  zero  velocity  measurements  were  defined  in  section  3.1,  and  will  now  be 
implemented  into  the  EKF  in  this  section.  Values  obtained  by  the  2-D  zero  velocity 
measurements  represent  the  directions  that  a  vehicle  is  known  to  be  not  moving  in. 
The  reason  for  taking  the  2-D  zero  velocity  measurements  as  the  orthogonal  vectors, 
of  the  camera’s  generated  vector,  is  because  the  true  magnitude  of  movement  is  an  un¬ 
known  element  of  a  monocular  camera.  A  monocular  camera  system  does  not  provide 
distance  measurements  to  a  detected  feature,  thus  leaving  the  system  as  a  direction  of 
movement  observation  only.  Overall  magnitude  of  movement  for  the  system  is  given 
by  the  INS’s  velocity  measurements.  Error  measurements  are  generated  from  the  INS 
velocity  measurements  in  the  north,  east,  and  down  direction  being  projected  onto 
the  2-D  zero  velocity  vectors  generated  by  the  monocular  camera  computer  vision  al¬ 
gorithm.  These  error  measurements,  generated  from  the  INS  velocity  measurements 
being  projected  onto  the  2-D  zero  velocity  vectors,  are  computed  by  the  dot  product 
as  seen  in: 

z  (4)  =  H(tk)x(tk)  +  v(tk)  (82) 
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happen.  Since  the  camera  measurement  is  taken  over  a  period  of  time,  and  can’t  be 
related  directly  to  a  specific  time  instance,  a  way  to  relate  the  measurement  to  a 
specific  INS  measurement  had  to  be  devised.  To  accomplish  this,  the  camera’s  2-D 
zero  velocity  measurement  was  rotated  into  the  navigation  frame  of  reference  using 
a  DCM  that  was  generated  by  the  INS  at  a  time  between  the  two  images.  In  turn, 
the  velocity  measurement  that  was  taken  from  the  INS  was  also  taken  at  the  same 
time.  This  means  that  there  was  a  slight  difference  in  the  time  the  error  measurement 
was  being  generated  for  and  the  actual  velocity  it  was  being  related  to.  Because  the 
dynamics  of  a  terrestrial  vehicle  are  not  very  high,  the  effects  of  doing  this  could  be 
considered  negligible. 

The  zero  velocity  that  was  generated  by  the  camera  measurement  was  then  used 
to  populate  the  measurement  observation  matrix  Himage(tk)  in  the  following  manner: 


H(i5image) 

i  n 

-L(l,n) 

1  n 

-L(l,e) 

1  n 

J-(M) 

image) 

1  n 

_  (2,n) 

1  n 

J-(2,e) 

1 

H 

(86) 


_  0(1x3)  H(1;jmage)(1;t,3)  0(12,3)  0(12,3)  0(12,3) 

l*- earner  a 

0(1x3)  !  H(2;image)(i23)  |  ^(1x3)  |  0(1x3)  {  0(ix3) 
where  the  values  H(i  image)  and  H(2, image)  are  are  placed  in  the  columns  that  corre¬ 
spond  to  the  velocity  states. 

Because  there  was  no  known  measurement  noise  for  this  type  of  measurement, 
a  test  had  to  be  run  to  solve  for  the  noise  that  was  associated  with  this  measure¬ 
ment.  Noise  for  the  system  was  computed  by  using  the  true  velocity  at  the  time  of 
a  computer  vision  measurement  and  reading  the  actual  measurement  value  that  was 
produced.  It  is  known  that: 


(87) 


z  (tk)  =  H(4)x(4)=0 


(88) 
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if  the  system  had  no  noise  associated  with  it  and  true  velocity  values  were  imple¬ 
mented,  the  measurement  should  be  equal  to  zero.  Knowing  that  there  is  some  noise 
associated  with  the  system,  the  equation  can  then  be  re-written  to  obtain  the  noise 
associated  with  the  computer  vision  measurements 


z  (tk)  =  v(4)  (89) 


By  taking  realization  over  an  entire  run,  1300  measurements  total,  a  statistical  rep¬ 
resentation  of  the  computer  vision  measurements  noise  could  be  obtained.  The  mea¬ 
surements  noise  was  then  associated  with  the  velocity  of  the  vehicle  and  pictorially 
represented  in  Figures  11  and  12. 
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Figure  11.  Unitless  measurement  noise  obtained  from  the  dot  product  of  the  2-D  zero 
velocity  vector  with  the  true  velocity  vector  for  the  east  direction  of  travel. 
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Figure  12.  Unitless  measurement  noise  obtained  from  the  dot  product  of  the  2-D  zero 
velocity  vector  with  the  true  velocity  vector  for  the  down  direction  of  travel. 


From  the  measurement  noise  values  obtained,  a  decent  approximation  of  the  cam¬ 
era’s  measurement  noise  could  be  made.  It  was  observed  that  the  noise  for  the  camera 
measurements  were  correlated  to  the  actual  speed  of  the  vehicle.  The  faster  the  ve¬ 
hicle  traveled,  the  less  noise  there  was  associated  with  the  measurement.  To  take 
this  into  account,  the  varying  sigma  values,  seen  in  Figure  13,  were  integrated  in  the 
system. 


Figure  13.  Calculated  noise  values  based  off  the  measured  noise  when  taking  vision 
sensor  measurements  with  the  true  velocity  measurements  being  supplied  to  the  system. 
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Noise  values  were  implemented  as  an  exponential  decay,  with  the  higher  values 
being  seen  at  low  velocities  and  lower  values  being  seen  at  a  higher  velocities.  Velocity 
of  the  system  was  determined  by  the  magnitude  of  the  INS’s  velocity  in  the  north, 
east  and  down  velocity  Finally,  the  covariance  of  the  measurement  was  described  as: 
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(90) 


where  the  crcameradown  and  o  cameraeast  were  determined  by  an  exponential  function  that 
was  driven  by  the  velocity  The  functions  that  were  used  for  this  were 


&  cameraeast 


0,75e(-0'Vl”"+”?+»))+o.2) 


(91) 


&  earner  a  down 


0.9e(-°-v^^I)+0-1) 


(92) 


When  the  magnitude  of  the  velocity  was  below  1.8  (m/s)  the  acamera  values  for  both 
the  east  and  the  down  direction  were  set  equal  to  0.99. 


3.5  GPS  Measurement  Update 

In  this  thesis,  pseudorange  measurements  from  different  satellites  were  used  to 
estimate  position.  Satellites  were  removed  to  simulate  a  GPS  degraded  environment. 
This  allowed  the  system  to  observe  different  outages  throughout  any  specific  run.  The 
end  result  was  a  system  that  could  have  any  number  of  satellites  in  view,  as  to  not  give 
a  full  GPS  solution,  and  rely  more  heavily  on  the  use  of  the  image  aided  measurements 
for  system  error  state  propagation.  In  order  to  correct  for  position  errors  without 
having  a  full  GPS  solution,  calculated  pseudoranges  from  the  users  position  were 
compared  to  actual  pseudoranges  given  by  the  satellite  [15].  The  measurement  model 
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for  a  single  GPS  measurements  found  in  [11]  can  be  seen  here 


Sz(tk)  =  H(tk)8x(tk)  +  v(tk) 


(93) 


8zp(tk)  —  piNs(tk )  —  PGPs(tk )  (94) 

where  5zp(tk )  is  the  error  in  pseudorange  at  the  time  of  measurement,  pi  ns  is  the 
estimated  pseudorange  from  the  inertial  navigation  INS,  and  Pgps  is  the  measured 
pseudorange  from  the  satellite  vehicle  (SV)  to  the  vehicle  GPS  receiver.  The  measured 
pseudorange  [11],  that  is  non-linear,  can  be  found  by 


PGPs(tk )  =  \[ (x(tk)  —  xGPS(tk)y  +  (y(tk)  —  yGPs(tk)y  +  (z(tk)  —  zaps{tk))2  +  8br 

(95) 


where  x,  y,  and  z  is  the  true  position  of  the  vehicle’s  receiver  in  the  ECEF  frame, 
xgps,  Vgps j  and  zqps  is  the  SV’s  position  in  the  ECEF  frame,  and  8br  =  c5tr  is  the 
range  error  in  meters  due  to  the  receiver’s  clock  bias  and  is  equal  to  the  speed  of  light, 
c,  multiplied  by  the  receiver’s  clock  offset, tr.  The  corrected  position  of  the  receiver  is 
then  expressed  as 


X  XINS  ~  bx 

V  =  yiNS  -  8y  (96) 

^  ZlNS  -  8z 

where  xins,  Pins,  and  zins  are  the  mechanization  of  the  outputs  and  8x,  5y,  and  Sz 
are  the  estimated  position  error.  After  applying  the  Taylor  series  expansion  around 
the  mechanized  output  and  linearizing  Equation  95,  the  linearized  measurement  [11] 
can  be  given  as 


Pins~ Pgps 


( XlNS_  —  XGPsKxins^  —  x)  +  (yiNS  —  yGPsH^INS^  —  y)  +  ( ZiNS_  ~  ^GPg)(^JjVS  ~  %) 

\/r(xni^-^GPsW~^TyiN^~^yGPsy~+T^iN^^GP^ 

(97) 
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A  line  of  sight  vector  from  the  satellite  to  the  receiver’s  position  can  then  be  defined 


as 
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(98) 


where  l j ns  is  the  line  of  sight  vector.  By  solving  for  the  estimated  position  error 
instead  of  the  corrected  position  seen  in  Equation  96,  you  get 


8x 

XINS  —  x 

8y 

vins  -  y 

5z 

ZlNS  ~  z 

(99) 


The  error  values  in  Equation  99  and  line  of  sight  values  in  Equation98  can  then  be 
substituted  into  Equation  97  to  get 


Szp  = 


lx.TNS  l 


xJNS  Ly,INS  lz,INS 


Sx 

5y 

8z 


—  8br 


(100) 


From  [11],  the  error  values  in  Equation  99  can  be  converted  from  ECEF  geodetic 
coordinates  to  linearized  ECEF  rectangular  coordinates  and  expressed  as 


Sx 

—  (Rn  +  h)  sin  p  cos  A  —  (Rn  +  h)  cost/?  sin  A  cost/?  cos  A 

Sip 

8y 

= 

—  (Rn  +  h)  sin</?sin  A  (Rn  +  h)  cos  </>cos  A  cosy?  sin  A 

SX 

5z 

(Rn(l  —  e2)  +  h)  cos  p  0  sin^ 

Sh 

where  Sp  is  the  error  in  latitude,  AA  is  the  error  in  longitude,  and  8h  is  the  error  in 
altitude.  From  Equation  97  and  Equation  101  the  observability  matrix  77(4 )  can  be 
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given  as 


Hp{tk)  = 


L 


l 


x,INS  <>y,INS  vzJNS 


—  (Rn  +  h )  sin  p  cos  A  —  ( Rn  +  h )  cos  p  sin  A  cos  p  cos  A 

—  (Rn  +  h)  sin^sin  A  (Rn  +  h)  cos<^cos  A  cos</?sinA 


(Rn(  1  —  e2)  +  h )  cos  p 
Finally,  the  measurement  equation  for  the  system  is  given  as 


0 


sin  9? 


(102) 


8zp{tk)  Hp(tk) 


8p 

S\ 

Sh 


8b  r  (tk) 


(103) 


The  covariance  of  the  measurement  was  defined  as 


R  = 


<j\ 


GPS 


(104) 


where  <jgps  is  the  standard  deviation  of  the  GPS  measurements  and  was  set  to  a 
constant  value  of  6  meters. 
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IV.  Results 


Results  that  were  obtained  for  the  simulations  utilizing  a  monocular  computer 
vision  algorithm,  GPS  aided  navigation,  and  monocular  computer  vision  algorithm 
and  GPS  aided  navigation  will  be  discussed  in  this  chapter.  Information  will  be 
presented  in  a  logical  order  showing  the  improvement  of  the  navigation  solution  due 
to  different  measurement  sources  being  available. 

4.1  Data  Sets 

Data  sets  that  were  used  for  this  thesis  are  presented  in  this  section.  A  collection  of 
points  generated  by  the  AFIT  Autonomous  Navigation  and  Technology  (ANT)  center 
during  the  All  Source  Positioning  Navigation  (ASPN)  program  were  used  as  test 
points  throughout  this  thesis.  The  test  points  generated  during  the  ASPN  program 
and  used  as  the  real  data  source  in  this  research,  were  defined  as  scenarios,  and  will 
be  referenced  as  scenarios  in  this  work.  There  were  a  total  of  four  different  scenarios 
that  were  used  to  test  the  results  of  a  monocular  camera  computer  vision  algorithm, 
IMU  sensor,  and  GPS  sensor  paired  in  different  configurations.  While  not  all  angles 
of  the  monocular  camera  view  were  available  during  the  test,  enough  diversity  in  the 
camera  view  was  given  to  make  comparisons  between  separate  runs  that  show  the 
benefits  and  drawbacks  of  different  monocular  camera  views. 

Scenario  2. 

Scenario  2’s  data  points  were  generated  by  a  vehicle  that  was  driving  around  the 
campus  of  Ohio  State  University  (OSU).  A  rural  area  is  depicted  in  this  scenario, 
meaning  there  were  sparse  buildings,  natural  foliage,  and  low  vehicle  and  pedestrian 
congestion.  GPS  was  not  adversely  affected  for  most  of  the  scenario,  other  than  the 
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periods  of  time  that  the  vehicle  was  crossing  or  sitting  under  a  bridge.  Camera  views 
available  for  Scenario  2,  were  a  left  and  forward  facing  camera.  The  horizontal  and 
vertical  trajectory  of  the  vehicle  can  be  seen  in  Figures  14  and  15. 


Figure  14.  Scenario  2  true  horizontal  tra-  Figure  15.  Scenario  2  true  vertical  trajec- 
jectory  tory 


Scenario  6. 

Scenario  6’s  data  points  were  collected  while  a  vehicle  was  driving  circles  in  a 
parking  lot.  There  were  no  buildings  or  natural  foliage  in  the  local  vicinity  of  this 
scenario.  Instead,  the  most  obstructive  points  were  generated  from  vehicles  that  were 
parked  sparsely  in  the  parking  lot.  GPS  was  available  during  the  entire  scenario,  and 
no  degradation  of  the  system  was  exhibited.  Camera  views  available  for  this  scenario, 
were  a  front  and  right  side  facing  camera.  The  horizontal  and  vertical  trajectory  of 
the  vehicle  can  be  seen  in  Figures  16  and  17. 
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Figure  16.  Scenario  6  true  horizontal  tra¬ 
jectory 


Figure  17.  Scenario  6  true  vertical  trajec 
tory 


Scenario  7. 

Scenario  7’s  data  points  were  collected  while  driving  around  the  downtown  section 
of  Columbus,  OH.  An  urban  environment  is  depicted  in  this  scenario,  meaning  there 
were  a  considerable  amount  of  large  buildings,  cross  traffic,  and  pedestrians  in  close 
proximity  of  the  vehicle.  GPS  was  degraded  for  most  of  the  run  due  to  multi-pathing 
and  signal  outages  caused  by  the  buildings.  Camera  views  that  were  available  during 
this  scenario,  were  a  front,  left  side,  and  right  side  facing  camera.  The  horizontal  and 
vertical  trajectory  of  the  vehicle  can  be  seen  in  Figures  18  and  19. 


Figure  18.  Scenario  7  true  horizontal  tra-  Figure  19.  Scenario  7  true  vertical  trajec 
jectory  tory 
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Scenario  17. 


Scenario  17’s  environment  was  represented  by  the  same  type  of  environment  de¬ 
picted  in  Scenario  7.  A  left  facing  camera  view  was  the  only  one  available  for  this 
scenario.  The  horizontal  and  vertical  trajectory  of  the  vehicle  can  be  seen  in  Fig¬ 
ures  20  and  21. 


Figure  20.  Scenario  17  true  horizontal  tra-  Figure  21.  Scenario  17  true  vertical  trajec- 
jectory  tory 


4.2  IMU  Only  Solution 

Relevance  of  IMU  aiding  is  developed  in  this  section.  While  each  scenario  will 
not  be  depicted  individually,  an  overall  representation  of  how  the  system  performs 
without  aiding  sensors  will  be  developed  here.  For  this  section,  Scenario  7’s  results  of 
an  unaided  IMU  will  be  visually  represented.  The  actual  trajectory  of  this  scenario 
was  annotated  in  Section  4.1.  The  horizontal  and  vertical  trajectories  estimated  by 
the  unaided  IMU  can  be  seen  in  Figures  22  and  23. 


53 


Figure  22.  IMU’s  estimated  horizontal  tra-  Figure  23.  IMU’s  estimated  vertical  trajec 
jectory  for  Scenario  7  with  no  aiding  mea-  tory  for  Scenario  7  with  no  aiding  measure 
surements.  ments 


As  expected,  the  solution  given  by  an  unaided  IMU  is  very  poor.  Measurements 
that  are  given  by  a  commercial  IMU  tend  to  have  a  lot  of  bias  added  to  them  and  can 
only  perform  well  for  very  short  periods  of  time  if  unaided.  While  the  trajectories  of 
the  system  do  not  provide  much  insight  into  what  is  happening,  the  position  error 
and  estimated  standard  deviation  by  the  filter  speaks  volumes  about  the  tuning  of 
the  system.  This  is  important  to  note,  because  system  tuning  will  play  a  role  in 
every  measurement  that  comes  into  the  system.  Values  obtained  for  position  error  in 
Scenario  7  can  be  seen  in  Figure  24. 
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Figure  24.  Scenario  7  IMU  position  error  with  filter  estimated  standard  deviation  with 
no  aiding  measurements. 


While  these  figures  are  not  enough  to  truly  describe  all  the  errors  seen  by  the 
system,  they  do  provide  a  visual  representation  of  what  is  going  on.  Position  errors 
seen  in  Figure  24  ,  for  a  simulated  commercial  grade  IMU,  match  those  that  are 
expected  from  a  commercial  grade  IMU  highlighted  in  [5]  and  [1] .  To  give  numerical 
values  that  can  be  compared  between  system  configurations,  the  approach  developed 
in  [15]  will  be  used,  in  which  the  distance  root  mean  square  (DRMS)  error  will  be 
used  to  describe  overall  performance  of  the  system.  The  mean  DRMS  error  is  defined 
by: 

DRMS  =  \/§kM±25  (105) 

V  n 
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where  n  is  the  number  of  measurement  in  the  epochs,  Xi  is  the  north  error  for  the 
'i!h  epoch  ,  and  xt  is  the  east  error  for  the  ith  epoch.  Because  there  were  variances 
in  the  distances  traveled,  errors  produced  by  each  simulated  IMU,  and  the  lengths 


of  each  run,  a  separate  DRMS  error  value  had  to  be  determined  for  each  scenario. 
The  DRMS  values  that  were  obtained  for  the  unaided  IMU  scenarios  can  be  seen  in 
Table  2 

Table  2.  DRMS  values  for  a  single  run  with  the  an  IMU  that  is  not  being  aided 


Scenario 

DRMS(m) 

Scenario  2  Commercial  IMU 

55165 

Scenario  2  Simulated  IMU 

170934 

Scenario  6  Simulated  IMU 

68066 

Scanario  7  Simulate  IMU 

443094 

Scenario  17  Simulated  IMU 

425145 

Table  2  shows  that  the  simulated  and  actual  commercial  grade  IMU  errors  do  not 
match  exactly.  This  does  not  raise  any  concern  though.  Solutions  provided  by  an  IMU 
can  perform  better  or  worse  than  expected  at  any  time,  this  particular  simulation  just 
happened  to  have  a  worse  performance  than  that  of  the  real  IMU.  Another  thing  to 
note  is  the  parameters  values,  that  were  given  by  the  vendor,  for  the  real  IMU  used 
in  Scenario  2.  The  parameters  values  for  the  real  IMU  were  lower  than  those  found 
in  Table  1,  meaning  the  IMU  should  perform  a  little  better  than  what  the  simulated 
IMU  did.  The  other  scenarios  show  better  and  worse  performance  due  to  the  duration 
of  the  run.  The  IMU  is  expected  to  get  exponentially  worse  as  the  system  operates 
without  aiding.  DRMS  values  will  be  given  for  each  sensor’s  pairing  and  used  to 
compare  the  system’s  performance  for  all  the  combination  of  sensors  that  are  run  in 
this  thesis. 


56 


4.3  GPS  Aided 


This  section  will  show  the  results  of  the  system’s  performance  when  aided  with 
different  grades  of  GPS  solution.  Grades  of  GPS  solution  will  be  determined  by  the 
number  of  SVs  in  sight  during  the  test  run.  The  GPS  will  be  restricted  to  a  specific 
amount  of  satellites  that  is  dictated  by  the  grade  of  GPS  required  and  the  number  of 
satellites  that  are  actually  available  in  the  scenario.  Sigma  values  given  for  the  error 
in  SV  measurements  have  been  set  as  6  meters  for  open  rural  areas  of  travel,  Scenarios 
2  and  6,  and  have  been  increased  to  7  meters  for  city  environments,  Scenarios  7  and 
17,  to  account  for  multi  path  errors  that  can  be  induced  into  the  system  due  to  the 
vehicles  surroundings. 

Performance  of  the  system  operating  with  one  SV  will  now  be  evaluated.  To 
depict  what  is  going  on  with  the  system,  the  horizontal  trajectory  of  the  scenario  is 
given  in  Figure  25. 


Figure  25.  Scenario  7  horizontal  trajectory  while  being  aided  by  1  SV. 

The  number  of  SVs  seen  during  the  duration  of  the  run,  and  the  particular  SV 
used  for  each  measurement  will  be  shown  in  Figures  26  and  27. 
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Figure  26.  Scenario  7  number  of  SVs  used  Figure  27.  Scenario  7  SV  being  used  to  gen- 
for  each  measurement.  erate  measurement. 

As  expected,  the  performance  of  the  system  is  poor  due  to  the  low  observability  of 
just  one  SV.  There  is  a  little  added  benefit  to  the  system  during  the  transition  from 
one  SV  to  the  next.  This  added  benefit  can  be  seen  when  the  position  is  scoped  back 
to  the  actual  vehicle’s  trajectory  seen  in  Figure  25,  and  it  is  directly  correlated  to  the 
times  that  the  system  transition  from  one  SV  to  another  seen  in  Figure  27.  Being 
that  the  data  is  real  and  satellites  come  and  go,  it  was  not  feasible  to  produce  a  real 
system  that  received  measurements  from  one  specific  SV  for  the  entire  scenario. 

The  actual  DRMS  values  for  all  the  scenario  operating  with  one  SV  can  be  seen 
in  Table  3. 

Table  3.  DRMS  values  for  IMU  being  aided  with  1  SV 


Scenario 

DRMS(m) 

Scenario  2  Commercial  IMU 

1516 

Scenario  2  Simulated  IMU 

3231 

Scenario  6  Simulated  IMU 

13324 

Scenario  7  Simulate  IMU 

14655 

Scenario  17  Simulated  IMU 

1283619 
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The  systems  performed  differently  due  to  the  number  of  SV  transitions  that  were 
encountered  and  the  duration  of  the  run.  Scenario  7  and  17  performed  the  worst 
because  of  their  consistency  of  a  specific  SV  being  viewable  for  the  duration  of  the 
scenario.  When  looking  at  Scenario  17  specifically,  the  system  had  a  degradation  in 
system  performance  when  aided  with  a  single  SV,  which  is  due  to  the  consistency  of 
a  single  SV  being  viewable  for  prolonged  periods  of  time.  From  Figure  28,  it  is  easy 
to  see  that  Scenario  17  gets  very  little  benefits  from  SVs  coming  in  and  out  of  view. 
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Figure  28.  Scenario  17  SV  being  used  to  generate  measurement  vs  time. 

System  performance  for  an  IMU  being  aided  with  2  SVs  will  now  be  evaluated. 
The  vehicle’s  horizontal  trajectory  is  shown  in  Figure  29,  and  the  number  of  SVs  in 
view  at  the  time  of  measurement  along  with  the  specific  SVs  used  for  the  measure¬ 
ments  can  be  seen  in  Figures  30  and  31. 
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East(m) 

Figure  29.  Scenario  7  horizontal  trajectory  while  being  aided  by  2  SVs. 


Figure  30.  Scenario  7  number  of  SVs  used  Figure  31.  Scenario  7  SV  being  used  to  gen- 
for  each  measurement.  erate  measurement  vs  time. 

The  same  natural  GPS  outages  seen  in  Figure  26  can  be  seen  in  Figure  30,  which 
shows  that  the  system  is  not  arbitrarily  negating  specific  measurements.  The  hor¬ 
izontal  trajectory  of  the  system  also  shows  a  drastic  improvement  compared  to  the 
trajectory  seen  when  the  system  was  operating  with  only  one  SV  in  view.  This  in¬ 
crease  in  performance  is  due  to  both  the  number  of  SVs  being  viewable  at  a  specific 
time  and  the  number  of  transitions  between  SVs  due  to  natural  outages. 
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How  the  system  performed  numerically  is  seen  in  Table  4. 


Table  4.  DRMS  values  for  IMU  being  aided  with  2  SVs 


Scenario 

DRMS(m) 

Scenario  2  Commercial  IMU 

2376 

Scenario  2  Simulated  IMU 

1124 

Scenario  6  Simulated  IMU 

151 

Scenario  7  Simulate  IMU 

225.8 

Scenario  17  Simulated  IMU 

164010 

All  of  the  systems  see  an  increase  in  performance  when  being  aided  with  2  SVs 
instead  of  1,  except  for  Scenario  2  operating  with  the  real  commercial  grade  IMU.  It 
was  noted,  during  the  early  stages  of  testing,  that  this  IMU  was  actually  performing 
worse  than  what  the  system  was  expected  to  do.  This  does  not  say  anything  about  the 
IMU  though,  because  the  expected  errors  for  the  system  are  generated  over  numerous 
runs  and  the  actual  system  performance  can  be  better  or  worse  than  what  is  expected 
at  any  given  time.  For  this  case,  the  system  is  performing  worse  than  what  is  expected, 
but  is  giving  feedback  to  the  filter  as  if  its  measurements  were  more  accurate  than 
what  is  being  produced. 

System  performance  will  now  be  evaluated  when  3  SVs  are  used  to  aid  the  IMU. 
The  trajectory  of  the  system  along  with  the  number  of  SVs  in  view  during  each 
measurement  can  be  seen  in  Figures  32,  33,  and  34. 


61 


200 


East(m) 


Figure  32.  Scenario  7  horizontal  trajectory  of  IMU  being  aided  by  3  SVs. 


Figure  33.  Scenario  7  number  of  SVs  view-  Figure  34.  Scenario  7  SVs  being  used  vs 
able  vs  time.  time. 

The  system  performs  very  well  when  only  3  SVs  are  available.  Normally,  3  satel¬ 
lites  would  not  be  expected  to  generate  a  good  solution,  due  to  unobservability  of  the 
four  states  that  are  being  estimated.  (For  a  simulation  example,  see  [15].)  In  this 
case,  however,  the  satellites  came  and  went  frequently  due  to  blockages,  so  the  actual 
satellites  being  tracked  included  more  than  3  satellites,  even  though  only  3  would  be 
tracked  in  any  one  measurement  epoch. 
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Numerical  results  for  the  system  operating  with  the  3  SVs  can  be  seen  in  Table  5. 

Table  5.  DRMS  values  for  IMU  being  aided  with  3  SVs 


Scenario 

DRMS(m) 

Scenario  2  Commercial  IMU 

14.7 

Scenario  2  Simulated  IMU 

10.4 

Scenario  6  Simulated  IMU 

6.7 

Scenario  7  Simulate  IMU 

14.7 

Scenario  17  Simulated  IMU 

11.59 

The  total  errors  for  the  system  are  drastically  reduced  when  a  third  satellite  is 
available  to  the  system.  Even  in  the  urban  environment  seen  in  Scenarios  7  and  17 
large  decrease  in  the  errors  are  observed.  This  is  due  to  a  low  number  of  outages 
caused  by  the  surrounding  buildings,  which  gave  way  for  a  pretty  clear  resolution  of 
the  position  given  by  the  GPS.  Results  for  an  IMU  being  aided  with  a  full  GPS 
solution  will  now  be  evaluated. The  horizontal  trajectory  of  the  system  is  given  in 
Figure  35,  and  the  number  of  SVs  available  vs  time  along  with  the  specific  SVs  being 
used  are  given  in  Figures  36  and  37. 
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Figure  35.  Scenario  7  horizontal  trajectory  of  IMU  being  aided  by  all  available  SVs. 


Figure  36.  Scenario  7  number  of  SVs  view-  Figure  37.  Scenario  7  SVs  being  used  vs 
able  vs  time.  time. 

As  expected,  the  system  performs  very  well  when  all  the  available  satellites  are 
used.  During  times  of  GPS  degradation,  the  performance  of  the  system  decreases,  but 
then  corrects  itself  when  more  satellites  become  available.  By  evaluating  the  velocity 
and  position  errors  with  their  respective  standard  deviation,  a  full  understanding  of 
how  the  system  performed  can  be  gained.  These  errors  can  be  seen  in  Figures  38  and 
39. 
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Figure  38.  Scenario  7  IMU  aided  by  all  Figure  39.  Scenario  7  IMU  aided  by  all 
available  SVs  associated  error  in  position  available  SVs  associated  error  in  velocity 
with  standard  deviation.  with  standard  deviation 


Error  in  the  system  position  and  velocity  is  minimal  during  the  time  of  a  full 
GPS  solution,  and  the  only  time  the  system  lacks  in  performance  is  during  the  GPS 
outages.  As  the  number  of  viewable  SVs  decrease,  the  error  in  the  system  steadily 
grows.  The  growth  in  the  position  and  velocity  error  correspond  directly  with  the 
decrease  of  viewable  SVs  seen  in  Figure  36.  Overall  results  for  the  system,  when  aided 
with  different  GPS  solutions,  can  be  seen  in  Table  6 


Table  6.  DRMS  value  comparison  for  the  IMU  being  aided  with  different  GPS  solutions 


Scenario 

0  SV  DRMS 

1  SV  DRMS 

2  SV  DRMS 

3  SV  DRMS 

Available  SV  DRMS 

Scenario  2  Com  IMU 

55165 

1516 

2376 

14.7 

9.9 

Scenario  2  Sim  IMU 

170934 

3231 

1124 

10.4 

4 

Scenario  6  Sim  IMU 

68066 

13324 

151 

6.7 

2.5 

Scenario  7  Sim  IMU 

443094 

14655 

225.8 

14.7 

7.7 

Scenario  17  Sim  IMU 

425145 

1283619 

164010 

11.59 

4.5 

From  the  values  given  in  Table  6,  it  is  easy  to  see  that  each  additional  SV  seen 
by  the  system  reduces  the  overall  DRMS  error  of  the  system.  The  two  scenarios  that 
showed  a  decrease  in  performance  were  affected  by  either  the  IMU  or  the  presence  of 
a  single  SV  for  a  prolonged  period  of  time. 
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4.4  Vision  Aided 


This  section  will  explain  the  results  obtained  when  aiding  the  IMU  with  the 
monocular  camera  computer  vision  algorithm.  To  stay  consistent  with  what  has 
already  been  presented,  this  section  will  display  the  figures  of  the  results  that  were 
obtained  during  Scenario  7’s  test,  but  numerical  DRMS  results  for  all  the  scenarios 
will  be  given.  Results  in  this  section  will  compare  the  performance  of  the  system  as 
different  monocular  camera  views  are  used  to  aid  the  IMU.  Monocular  camera  views 
that  will  be  presented  in  this  section  are  going  to  be  the  forward  facing,  left  side 
facing,  and  right  side  facing  views. 


Forward  Facing  Camera. 

This  system  utilizes  a  camera  that  points  in  the  vehicle’s  forward  direction  of 
movement.  The  horizontal  trajectory  seen  in  Figure  40  and  the  heading  seen  in 
Figure  41  illustrate  the  performance  of  the  system  when  an  IMU  is  aided  with  a 
forward  facing  vision  sensor. 


Figure  40.  Scenario  7  estimated  vs  true 
horizontal  trajectory  with  a  forward  facing 
monocular  camera  aiding  an  IMU. 


Figure  41.  Scenario  7  estimated  vs  true 
heading  with  a  forward  facing  monocular 
camera  aiding  an  IMU. 
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Figure  42.  Scenario  7  tilt  error  vs  time  for  Figure  43.  Scenario  7  true  and  estimated 
a  forward  facing  monocular  camera  aiding  magnitude  of  velocity  vs  time  for  forward 
an  IMU.  facing  monocular  camera  aiding  an  IMU. 


It  is  clearly  seen  that  the  system  understands  when  the  vehicle  is  observing  a 
change  in  heading.  From  Figure  42,  the  tilt  error  that  is  seen  in  the  system  can  be 
observed.  As  the  vehicle  continues  to  travel,  the  yaw  tilt  error  of  the  system  continues 
to  grow,  which  contributes  to  the  difference  in  the  trajectory  and  heading  observed. 
While  the  system  seems  to  do  really  well  observing  changes  in  direction,  the  actual 
magnitude  of  movement,  seen  in  Figure  43,  is  less  observable.  The  inability  of  the 
system  to  sense  the  actual  amount  of  movement  is  expected  though.  The  monocular 
camera  system  is  unable  to  detect  the  range  to  a  feature  that  is  detected,  which  leads 
to  the  lack  of  observability  of  the  vehicle’s  actual  velocity.  The  issue  with  the  velocity 
was  further  exacerbated  by  the  filter  using  previously  recorded  velocity  values,  from 
periods  of  time  that  occurred  1/8  of  a  second  before  the  second  image  is  actually 
being  taken,  as  inputs  into  the  monocular  computer  vision  algorithm.  This  was  due 
to  the  imagery  sensor  taking  a  measurement  over  a  quarter  of  a  second,  and  not  being 
directly  related  to  the  velocity  measurement  that  was  given  when  the  second  image 
was  taken.  This  will  undoubtedly  create  errors  that  will  steadily  propagate  and  grow 
as  the  vehicle  continues  to  move.  Numerical  results  for  all  of  the  scenarios  that  had 
a  forward  facing  camera  can  be  seen  in  Table  7 
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Table  7.  DRMS  Values  for  a  forward  facing  monocular  camera  aiding  an  IMU 


Scenario 

Forward  DRMS(m) 

Scenario  2  Commercial  IMU 

125.4 

Scenario  2  Simulated  IMU 

289.2 

Scenario  6  Simulated  IMU 

57.6 

Scenario  7  Simulated  IMU 

509.4 

Side  Facing  Camera. 

This  section  will  show  the  results  for  both  the  left  and  right  side  facing  vision 
sensor  aiding  the  IMU.  These  two  side  facing  sensors  were  put  together  because  their 
results  were  similar.  The  horizontal  trajectories  that  were  generated  by  both  the  right 
and  left  side  facing  cameras  can  be  seen  in  Figures  44  and  45. 


Figure  44.  Scenario  7  horizontal  trajectory 
with  a  left  facing  monocular  camera  aiding 
an  IMU. 


Figure  45.  Scenario  7  horizontal  trajectory 
with  a  right  facing  monocular  camera  aiding 
an  IMU. 


While  the  forward  facing  monocular  camera  does  a  good  job  of  picking  up  vari¬ 
ations  in  the  vehicle’s  yaw,  the  side  facing  monocular  camera  can  not  give  the  same 
results.  Side  facing  monocular  cameras  do  not  see  features  radiating  outward  from  the 
center  of  the  camera  view  like  the  forward  facing  monocular  camera  does.  Instead, 
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side  facing  monocular  cameras  see  the  features  moving  in  a  horizontal  trajectory 
across  the  image  plain.  This  phenomenon  can  be  seen  in  Figures  46  and  47,  where 
the  side  facing  monocular  camera  has  horizontal  lines  connecting  all  the  features,  and 
the  forward  facing  monocular  camera  has  lines  that  start  radiating  vertically  to  their 
matched  features.  This  is  an  issue  since  all  horizontal  movement  leads  to  complica¬ 
tions  when  the  vehicle  encounters  a  turn.  While  a  forward  facing  monocular  camera 
would  start  seeing  a  majority  of  the  features  moving  horizontally  across  the  plain,  the 
side  facing  monocular  camera  sees  the  features  acting  in  the  same  way  that  it  has  for 
the  entire  scenario.  However,  the  features  represented  in  the  side  facing  monocular 
camera  will  give  a  better  insight  into  the  change  in  pitch  that  has  been  encountered 
by  the  vehicle. 


Figure  46.  Scenario  7  side  facing  monocular  camera  feature  observation  in  two  sequen¬ 
tial  photos. 
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Figure  47.  Scenario  7  forward  facing  monocular  camera  feature  observation  in  two 
sequential  photos. 


The  two  side  facing  monocular  cameras  were  able  to  detect  a  similar  number  of 
features  in  their  respective  held  of  vision.  A  comparison  of  the  number  of  features 
detected  by  each  camera  at  different  velocities  can  be  seen  in  Figures  48  and  49. 


0  5  10  15  20 


Magnitude  of  Velocity  (m/s) 

Figure  48.  Scenario  7  features  in  view  of 
left  facing  monocular  camera  vs  velocity  of 
the  vehicle. 


Magnitude  of  Velocity  (m/s) 


Figure  49.  Scenario  7  features  in  view  of 
right  facing  monocular  camera  vs  velocity 
of  the  vehicle. 


Similar  feature  detection  between  the  two  cameras  may  not  be  encountered  if 
vehicle  speeds  were  to  increase  during  the  scenario  runs.  Vehicle  speeds  hardly  went 
above  15  m/s  due  to  the  vehicle  operating  in  a  city  environment.  If  speeds  were  to 
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increase,  and  the  proximity  of  the  features  in  view  (i.e.,  buildings)  did  not  change, 
the  system  would  probably  not  do  as  well  because  features  in  two  sequential  images 
would  move  in  and  out  of  image  view  too  quickly  for  the  camera  to  capture  it.  There 
were  a  few  instances  when  the  camera  picked  up  a  large  amount  of  features  at  higher 
velocities,  but  this  is  only  due  to  the  fact  that  a  clearing  of  the  buildings  normally  in 
view  was  encountered  (i.e.,  the  vehicle  drove  through  an  intersection  and  the  depth 
of  the  image  drastically  increased). 

Errors  in  the  position,  velocity,  and  tilt  encountered  by  the  system  can  be  seen  in 
Figures  50,  51,  and  52. 
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Figure  50.  Scenario  7  error  in  position  for 
the  left  facing  monocular  camera  aiding  the 


IMU. 


Figure  51.  Scenario  7  error  in  velocity  for 
the  left  facing  monocular  camera  aiding  the 


IMU. 


Figure  52.  Scenario  7  tilt  error  for  the  left  facing  monocular  camera  aiding  the  IMU. 
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Error  for  position  and  the  velocity  of  the  system  stayed  within  the  thresholds  of 
what  the  predicted  error  for  the  system  was  thought  to  be.  The  values  that  fall  out¬ 
side  of  the  error  in  the  north  position  are  related  to  vehicle’s  direction  of  travel.  As 
mentioned  before,  the  vision  sensor  has  no  perception  of  total  movement  encountered 
in  the  forward  direction,  and  can  only  really  constrain  the  lateral  movement  of  the 
vehicle.  So,  anytime  the  vehicle  moves  directly  forward  in  a  specific  direction,  north 
or  east,  the  error  in  that  direction  is  going  to  grow  considerably  more  than  what  is 
normally  expected.  Tilt  error  of  the  system  is  considerable,  as  expected  for  a  side 
facing  camera.  The  ability  of  the  side  facing  camera  to  detect  a  turn  is  minimal,  due 
to  the  fact  that  all  the  features  are  moving  in  the  same  manner  as  they  have  always 
been  seen  moving. 

Overall,  the  two  side  facing  cameras  did  produce  a  noticeable  decrease  in  position 
and  velocity  error  over  a  commercial  IMU  operating  on  its  own,  and  there  was  not 
noticeable  difference  from  DRMS  error  values  produced  by  the  forward  facing  camera. 
Performance  of  the  different  facing  vision  sensors  can  be  seen  in  Table  8. 

Table  8.  DRMS  values  for  the  different  monocular  cameras  aiding  the  IMU. 


Scenario 

L  Side  DRMS 

R  Side  DRMS 

Forward  DRMS 

Scenario  2  Com  IMU 

184.0 

N/A 

125.4 

Scenario  2  Sim  IMU 

173.9 

N/A 

289.2 

Scenario  6  Sim  IMU 

N/A 

115.7 

57.6 

Scenario  7  Sim  IMU 

540.4 

362.7 

509.4 

Scenario  17  Sim  IMU 

567.9 

N/A 

N/A 
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4.5  IMU  aided  with  Vision  and  GPS 


This  section  will  show  the  results  that  were  obtained  when  aiding  the  IMU  with 
a  monocular  computer  vision  algorithm  and  GPS  simultaneously.  While  the  poses 
of  the  monocular  cameras  for  the  scenario  will  not  be  changed,  the  number  of  SVs 
aiding  the  system  will  change.  This  section  will  further  highlight  the  importance  and 
usability  of  aiding  an  IMU  with  a  monocular  computer  vision  algorithm  when  GPS 
is  degraded. 

The  trajectory  of  the  system  when  a  single  SV  paired  with  the  monocular  com¬ 
puter  vision  algorithm  is  used  to  aid  the  IMU  can  be  seen  in  Figure  53. 
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Figure  53.  Scenario  7  single  SV  paired  with  the  monocular  camera  computer  vision 
algorithm  to  aid  the  IMU. 

While  a  single  SV  has  problems  estimating  the  trajectory  because  of  bad  infor¬ 
mation  provided  by  the  IMU,  the  addition  of  the  camera  provides  a  relative  position 
reference  to  the  system.  This  makes  updates  from  the  SV  more  accurate  and  provides 
for  a  better  solution.  The  position,  velocity,  and  tilt  errors  given  by  the  system  can 
be  seen  in  Figures  54,  55,  and  56. 
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Figure  54.  Scenario  7  error  in  position  with 
1SV  and  the  monocular  camera  computer 
vision  algorithm  aiding  the  IMU. 


Figure  55.  Scenario  7  error  in  velocity  with 
1SV  and  the  monocular  camera  computer 
vision  algorithm  aiding  the  IMU. 


0  100  200  300  400  500  600 

Time(s) 

Figure  56.  Scenario  7  tilt  error  with  1SV  and  the  monocular  camera  computer  vision 
algorithm  aiding  the  IMU. 


While  the  system  is  still  not  optimal,  the  decrease  in  position,  velocity,  and  tilt 
errors  can  be  observed  compared  to  the  two  system  operating  independently.  Heading 
tilt  errors  that  were  observed  before  with  the  vision  sensor  acting  independently  have 
been  reduced  due  to  the  good  position  updates  that  are  occasionally  received  by  the 
GPS.  Results  for  all  the  scenarios  that  have  a  single  SV  paired  with  the  monocular 
computer  vision  algorithm  to  aid  the  IMU  can  be  seen  in  Table  9. 
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Table  9.  DRMS  values  for  a  single  SV  paired  with  the  monocular  camera  computer 
vision  algorithm  to  aid  the  IMU 


Scenario 

L  Side  DRMS(m) 

R  Side  DRMS(m) 

Forward  DRMS(m) 

Scenario  2  Com  IMU 

140.3 

N/A 

111.3 

Scenario  2  Sim  IMU 

80 

N/A 

88.5 

Scenario  6  Sim  IMU 

N/A 

43.4 

30.9 

Scenario  7  Sim  IMU 

247.5 

248.3 

213.1 

Scenario  17  Sim  IMU 

180.1 

N/A 

N/A 

The  number  of  SVs  seen  by  the  system  will  now  be  increased  to  two  and  will 
continue  to  be  paired  with  themonocular  computer  vision  algorithm  to  aid  the  IMU. 
The  horizontal  trajectory  for  the  paired  sensors  aiding  the  IMU  in  this  case  can  be 
seen  in  Figure  57. 


Figure  57.  Scenario  7  two  SVs  paired  with  the  monocular  camera  computer  vision 
algorithm  to  aid  the  IMU. 


By  increasing  the  number  of  SVs  to  two,  the  performance  of  the  system,  compared 
to  monocular  camera  computer  vision  algorithm  only  and  GPS  only  aiding,  gradually 
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increases.  While  there  are  still  periods  of  natural  GPS  outages  encountered,  the 

monocular  camera  computer  vision  algorithm  now  keeps  the  system’s  position  and 

velocity  errors  from  growing  like  it  would  if  it  had  no  additional  aiding.  Each  of  the 

other  scenarios  performance  results,  with  two  SVs  and  vision,  can  be  seen  in  Table  10. 

Table  10.  DRMS  values  for  two  SVs  paired  with  the  monocular  camera  computer  vision 
algorithm  to  aid  the  IMU 


Scenario 

L  Side  DRMS(m) 

R  Side  DRMS(m) 

Forward  DRMS(m) 

Scenario  2  Com  IMU 

52.2 

N/A 

29.6 

Scenario  2  Sim  IMU 

54.4 

N/A 

71.3 

Scenario  6  Sim  IMU 

N/A 

25.6 

16.8 

Scenario  7  Sim  IMU 

91.8 

152.4 

151.5 

Scenario  17  Sim  IMU 

193 

N/A 

N/A 

The  number  of  SVs  viewable  by  the  system  will  now  be  increased  to  3,  while  still 
being  aided  with  vision.  The  actual  trajectory  for  the  system  can  be  seen  in  Figure  58. 


Figure  58.  Scenario  7  3  SVs  paired  with  the  monocular  camera  computer  vision  algo¬ 
rithm  to  aid  the  IMU. 
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At  this  point,  the  system  shows  a  trajectory  that  could  almost  match  that  which 
would  be  expected  with  a  full  GPS  solution.  The  system  does  a  better  job  of  track¬ 
ing  the  actual  horizontal  trajectory  even  when  the  GPS  is  degraded  to  less  than  3 
available  SVs  available.  The  monocular  camera  computer  vision  algorithm  is  able  to 
provide  a  better  solution  during  times  of  GPS  degradation  due  to  the  better  initial¬ 
ization  points  that  have  been  created  by  the  GPS.  This  drastically  reduces  the  tilt 
errors  that  would  slowly  degrade  the  monocular  camera  computer  vision  algorithm 
over  time.  Each  of  the  other  scenarios  performance  results,  with  two  SVs  and  the 
monocular  computer  vision  algorithm,  can  be  seen  in  Table  11. 


Table  11.  DRMS  values  for  three  SVs  paired  with  the  monocular  camera  computer 
vision  algorithm  to  aid  the  IMU 


Scenario 

L  Side  DRMS(m) 

R  Side  DRMS(m) 

Forward  DRMS(m) 

Scenario  2  Com  IMU 

15.5 

N/A 

14.5 

Scenario  2  Sim  IMU 

7.9 

N/A 

7.4 

Scenario  6  Sim  IMU 

N/A 

13.7 

5.8 

Scenario  7  Sim  IMU 

14.5 

11.2 

24.2 

Scenario  17  Sim  IMU 

10.5 

N/A 

N/A 

From  the  results,  it  seems  as  if  the  side  facing  monocular  cameras  give  better 
results  than  the  forward  facing  monocular  camera  in  the  urban  environments,  and 
forward  facing  monocular  cameras  give  better  results  in  rural  open  areas.  This  is  due 
to  the  features  that  are  seen  by  the  camera  during  the  scenarios.  Side  facing  cameras 
may  have  a  better  recognition  of  features  in  a  urban  environment  than  they  do  in 
a  rural  environment  because  the  object  are  man  made  and  have  a  lot  more  distinct 
characteristics  than  what  natural  foliage  may  have.  The  traffic  patterns  seen  in  a 
city  may  also  disrupt  the  forward  facing  camera  by  having  features  that  are  detected 
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being  associated  with  a  car  that  is  turning.  These  vehicle  would  then  tell  the  vision 
sensor  that  it  is  in  a  turn  when  it  is  actually  going  straight. 

Finally,  a  full  GPS  solution  is  paired  with  the  monocular  camera  computer  vi¬ 
sion  algorithm  to  aid  the  IMU.  The  actual  trajectory  for  the  system  can  be  seen  in 
Figure  59 


Figure  59.  Scenario  7  four  or  more  SVs  paired  with  the  monocular  camera  computer 
vision  algorithm  to  aid  the  IMU. 


Again,  unavoidable  GPS  outages  to  the  system  are  experienced,  but  the  monocular 
computer  vision  algorithm  helps  drastically  reduce  the  overall  degradation  of  the 
system.  The  errors  in  the  horizontal  trajectory  that  were  seen  in  Figure  35  are 
now  reduced,  and  the  vehicle’s  estimated  trajectory  is  now  closer  to  what  the  actual 
trajectory  was.  The  errors  for  both  the  position  and  velocity  can  be  seen  in  Figures  60 
and  Figure  61. 
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Figure  60.  Scenario  7  with  all  available  SVs 
paired  with  the  monocular  camera  com¬ 
puter  vision  algorithm  to  aid  the  IMU  po¬ 
sition  error  with  standard  deviation. 


Figure  61.  Scenario  7  with  all  available  SVs 
paired  with  the  monocular  camera  com¬ 
puter  vision  algorithm  to  aid  the  IMU  ve¬ 
locity  error  with  standard  deviation. 


When  compared  to  the  position  and  velocity  error  values  of  just  GPS  aiding  the 
IMU,  seen  in  Figures  38  and  39,  the  decrease  in  errors  seen  during  GPS  outages  can 
be  observed.  Performance  for  all  the  systems  being  aided  with  GPS  and  vision  can 
be  seen  in  Table  12. 


Table  12.  DRMS  value  comparison  for  the  IMU  being  aided  with  GPS  and  the  monoc¬ 
ular  camera  computer  vision  algorithm. 


0  SV  DRMS 

1  SV  DRMS 

2  SV  DRMS 

3  SV  DRMS 

Available  SV  DRMS 

Scenario  2  Com  IMU  Forward  Cam 

125.4 

111.3 

29.6 

14.5 

10.8 

Scenario  2  Com  IMU  L  Cam 

184.0 

140.3 

52.2 

15.5 

11.2 

Scenario  2  Sim  IMU  Forward  Cam 

289.2 

88.5 

71.3 

7.4 

3.6 

Scenario  2  Sim  IMU  L  Cam 

173.9 

80 

54.4 

7.9 

4.2 

Scenario  6  Sim  IMU  Forward  Cam 

57.6 

30.9 

16.8 

5.8 

2.9 

Scenario  6  Sim  IMU  R  Cam 

115.7 

43.4 

25.6 

13.7 

3.1 

Scenario  7  Sim  IMU  Forward  Cam 

509.4 

213.1 

151.5 

24.2 

8.1 

Scenario  7  Sim  IMU  L  Cam 

540.4 

247.5 

91.8 

14.5 

7.8 

Scenario  7  Sim  IMU  R  Cam 

362.7 

248.3 

152.4 

11.15 

8.7 

Scenario  17  Sim  IMU  L  Cam 

567.9 

180.1 

193 

10.5 

4.9 
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4.6  Summary 


Performance  of  the  system  is  greatly  increased  when  using  the  monocular  camera 
computer  vision  algorithm  to  aid  the  system  of  a  GPS  denied  or  limited  environment. 
A  comparison  of  all  the  results  obtained  in  this  thesis  can  be  seen  in  Table  13. 


Table  13.  DRMS  value  comparison  for  all  sensor  combinations. 


0  SV  DRMS 

1  SV  DRMS 

2  SV  DRMS 

3  SV  DRMS 

Available  SV  DRMS 

Scenario  2  Com  IMU 

55165 

1516 

2376 

14.7 

9.9 

Scenario  2  Com  IMU  Forward  Cam 

125.4 

111.3 

29.6 

14.5 

10.8 

Scenario  2  Com  IMU  L  Cam 

184.0 

140.3 

52.2 

15.5 

11.2 

Scenario  2  Sim  IMU 

170934 

3231 

1124 

10.4 

4 

Scenario  2  Sim  IMU  Forward  Cam 

289.2 

88.5 

71.3 

7.4 

3.6 

Scenario  2  Sim  IMU  L  Cam 

173.9 

80 

54.4 

7.8 

4.2 

Scenario  6  Sim  IMU 

68066 

13324 

151 

6.7 

2.5 

Scenario  6  Sim  IMU  Forward  Cam 

57.6 

30.9 

16.8 

5.8 

2.9 

Scenario  6  Sim  IMU  R  Cam 

115.7 

43.4 

25.6 

13.7 

3.1 

Scenario  7  Sim  IMU 

443094 

14655 

225.8 

14.7 

7.7 

Scenario  7  Sim  IMU  Forward  Cam 

509.4 

213.1 

151.5 

24.3 

8.1 

Scenario  7  Sim  IMU  L  Cam 

540.4 

247.5 

91.8 

14.5 

7.8 

Scenario  7  Sim  IMU  R  Cam 

362.7 

248.3 

152.4 

11.2 

8.7 

Scenario  17  Sim  IMU 

425145 

1283619 

164010 

11.6 

4.5 

Scenario  17  Sim  IMU  L  Cam 

567.9 

180.1 

193 

10.5 

4.9 

As  mentioned  in  [15],  which  was  based  off  of  all  simulated  measurements,  the 
system  acts  like  it  has  gained  another  SV  by  introducing  the  monocular  camera  com¬ 
puter  vision  algorithm  into  the  system.  While  this  thesis  did  not  show  that  an  IMU 
aided  by  a  monocular  camera  computer  vision  algorithm  alone  can  create  a  reliable 
solution,  it  did  show  that  this  solution  is  a  great  aid  when  operating  in  limited  GPS 
areas. 

This  thesis  also  shows  that  there  is  a  negligible  difference  between  the  naviga¬ 
tion  solution  with  a  side  facing  camera  and  a  forward  facing  camera.  In  general,  the 
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number  of  features  presented  to  the  system  was  enough  to  give  a  trajectory  solution. 
Even  with  putting  a  limit  on  the  number  of  features  required  to  obtain  a  solution, 
which  was  set  at  20  features  for  this  research,  the  test  cases  had  very  few  instances 
that  the  monocular  camera  computer  vision  algorithm  solution  was  not  presented  to 
the  system. 

Finally,  the  urban  environments,  depicted  in  Scenarios  7  and  17,  had  little  degra¬ 
dation  on  the  overall  systems  performance.  While  cross  traffic,  pedestrians,  and 
proximity  to  buildings  hindered  the  vision  only  navigation  performance,  the  vision 
only  system  performed  better  than  a  single  SV  being  used  to  aid  the  system.  The 
performance  of  the  system  still  saw  a  performance  increase  of  one  additional  SV  with 
the  use  of  a  the  monocular  camera  computer  vision  algorithm  aiding  system  and  GPS. 
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V.  Conclusion  and  Summary 


The  final  conclusion  and  future  works  suggested  by  the  work  presented  in  this  will 
be  drawn  in  this  chapter. 

5.1  Conclusion 

Use  of  a  monocular  camera  computer  vision  algorithm,  as  an  aiding  sensor  to  the 
overall  navigation  solution,  was  presented  in  this  thesis.  Instead  of  using  the  normal 
eight  point  algorithm  to  determine  the  fundamental  matrix  and  subsequently  the 
rotation  from  that,  the  IMU’s  measured  rotation  values  were  used  to  constrain  the 
features  detected  by  a  monocular  camera  to  a  translation  only  value.  Regardless  of  the 
orientation  of  the  monocular  camera,  similar  final  results  were  obtained  for  the  left, 
right,  and  forward  facing  cameras.  Furthermore,  the  introduction  of  each  new  satellite 
to  the  system  only  increased  the  overall  accuracy  of  the  system.  Confirming  what 
was  already  said  in  [15],  the  monocular  computer  vision  algorithm,  when  aided  with 
GPS,  gave  solutions  that  were  equal  to  those  found  of  a  system  with  one  additional 
SV.  For  example,  a  system  with  one  SV  and  a  monocular  camera  computer  vision 
algorithm  gave  results  similar  to  a  system  with  two  SVs  and  no  monocular  camera 
computer  vision  algorithm.  It  is  important  to  note  that  the  system  did  see  different 
SVs  during  a  run,  which  gave  it  a  little  better  solution  than  if  it  tracked  the  same 
two  SVs.  During  time  periods  of  SVs  coming  in  and  out  of  view,  the  system  gets 
a  better  solution  because  it  now  has  more  SVs  in  view.  Scenario  17  shows  how  the 
system  acts  when  the  SV  being  viewed  is  held  constant.  The  solution  for  GPS  and 
IMU  only  solution  in  this  case  is  worse  than  any  of  the  other  test  runs. 
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5.2  Future  Work 


As  mentioned  earlier,  there  were  some  discrepancies  that  may  have  been  intro¬ 
duced  into  the  system  because  the  monocular  camera  computer  vision  algorithm 
measurements  were  applied  at  time  after  the  measurements.  Instead  of  applying  the 
measurements  at  the  time  of  the  second  image  to  a  value  that  the  IMU  measured 
half  way  between  the  two  image  time,  a  delayed  state  filter  should  be  implemented. 
This  will  reduce  the  small  errors  that  are  constantly  being  added  to  and  propagated 
forward.  Currently,  the  error  in  the  system  will  continually  grow  as  long  as  the  system 
is  running. 

Another  thing  that  was  not  included  in  this  work  was  batch  updating.  While  the 
images  of  the  system  don’t  have  to  be  stored  to  do  a  batch  update,  the  translation 
obtained  from  the  images  can  be  easily  stored  without  taxing  the  system’s  memory. 
Each  new  measurement  that  is  given  only  adds  a  little  more  observability  to  the  sys¬ 
tem. 

Determining  when  the  system  is  actually  stopped  is  also  very  important.  Because 
of  the  drift  errors  in  the  IMU  and  the  inability  of  the  monocular  camera  computer 
vision  algorithm  to  detect  when  the  vehicle  is  stopped,  position  errors  will  always  be 
encountered  when  the  vehicle  is  stationary.  The  reason  behind  the  movement  detected 
by  the  monocular  camera  computer  vision  algorithm  is  that  the  sensor  interprets  sta¬ 
tionary  features  as  movement  in  the  forward  direction  only.  By  supplementing  the 
system  with  a  zero  velocity  measurement,  the  system  can  be  constrained  to  no  move¬ 
ment  when  the  monocular  camera  computer  vision  algorithm  system  does  not  see 
features  moving.  This  will  keep  the  system  from  slowly  drifting  away  when  stopped. 

Lastly,  the  orientation  of  a  ground  facing  monocular  camera  should  be  tested  with 
the  measurement  equations  established  in  this  thesis.  By  implementing  this  camera, 
the  system  will  experience  fewer  outages  due  to  sun  light  saturating  the  sensors.  The 
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other  advantages  is  the  distance  to  features  will  be  more  observable.  By  knowing  the 
height  above  the  ground,  less  error  of  the  system  associated  with  observability  in  the 
camera’s  forward  direction  or  z-direction  can  be  experienced. 
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